//  LivingSpot.cpp

//#define NO_HARDWARE

#include "WProgram.h"
#include "LivingSpot.h"

// calibration position (OFF CENTER to avoid specular reflexion from the surface - in particular if there is a glass, in which case a black disk may be needed)
#define POS_AD_MIRROR_CALIB_X (1.0*(MAX_AD_MIRRORS-MIN_AD_MIRRORS)/3) 
#define POS_AD_MIRROR_CALIB_Y (1.0*(MAX_AD_MIRRORS-MIN_AD_MIRRORS)/3) 

float _rawScanData[MAX_SACCADE_SAMPLES]; 

// ===================================================================================================

//-----------------constructors & initializers ----------------
void LivingSpot::Init(float posX, float posY, float saccadeRadius, int numSample, int id) {
        
        //attention! memory leak otherwise! difficult to debug on a microcontroller... 
        if (numSample>MAX_SACCADE_SAMPLES) numSample= MAX_SACCADE_SAMPLES; 
  
        // **** sound generation mode (see update method):  
        touchSoundMode=SUPER_BASS;
        collisionSoundMode=SUPER_BASS; 
        countourSoundMode=SUPER_BASS;
  
	// **** spot identifier
	identifier=id; 

	// **** Default behaviour and "alive" mode:
	alive=false;
	behaviourMode=NEWTON_DYNAMICS; // can be: SIMPLE_BOUNCING, FAST_BOUNCING, NEWTON_DYNAMICS, COUNTOUR_FOLLOWING, SNELL_LAW

	//**** Default values: 
	Az_Default=CENTER_AD_MIRROR_X; 
	El_Default=CENTER_AD_MIRROR_Y;  // Special position (goes to it when in RESET_POSITION behaviour)

        // MODELVIEW parameters (in the future, make a matrix with associated methods for creating it - identity, rotation, etc):
        // Position of the saccade: 
        Az=Az_Default; El=El_Default;
        // Rotation: 
        saccadeRotation=0; // in radians (to accelerate computation because we don't need to convert from deg to rad before calling trigonometric functions). 
        // Size: 
         // Initial aperture: 
        //Aper_Degree=180.0/PI*atan(R_cm_REF/depth_Range_REF); // initialization of the current angular aperture of the saccade 
	//Aper_DA=1.0*Aper_Degree/IO.AD_to_Deg_MIRROR_X; // initialization of the current angular aperture of the saccade (card units).
        setAperSaccade(saccadeRadius); // this will set the initial value of Aper_DA and Aper_Degree (i.e, the size of the drawing, or the radius of the circular saccade in case of a circular one). 
        
        // CREATION OF THE DEFAULT CIRCULAR SACCADE (i.e., scanning/displaying trajectory): 
	createCircularSaccade(numSample); // NOTE: this will set the initial (default) value of _numSamplesCircularSaccade	

        //**** Various flags default values, assuming that default calibration matches experimental conditions: 
        Proper_Calibration_Flag=GOOD_CALIBRATION; // default calibration. //BAD_CALIBRATION, FAIR_CALIBRATION, GOOD_CALIBRATION
        Proper_Intensity_Flag=APPROPRIATE_STRENGH; //TOO_WEAK, APPROPRIATE_STRENGH, SATURATED_SIGNAL 
        Proper_Distance_Flag=PROPER_RANGE; // TOO_FAR, PROPER_RANGE, TOO_CLOSE 
        Proper_Contrast_Flag=GOOD_CONTRAST; //GOOD_CONTRAST, BAD_CONSTRAST

	//**** Mirror and kinematic initialization: 
	//IO.setMirrorsXY_AD(Az, El); // REM: IO object is instantiated GLOBALLY

        // **** Intensity/distance model:   **************************   !!!!!!!!!!!!!!!!!! TO CHECK !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	Distance_Intensity_Model=ORDER_1_CORRECTED_LENS;
	exp_Cos=17; // cos exponent in the case of ORDER_0_CORRECTED_LENS model.
         // range_Adjust; // DEPRECATED!!
	PD_X=-2.5; PD_Y=5.5; // // in cm, position of photodetector in the X/Y plane (values for the new prototype (17.7.2010): (-2.5, 5.5))
	// The following three parameters are computed in the program DistanceCm_To_mW.m, out of linear regression on measured data:
	// Pre-defined parameters (DistanceCm_To_Volts.m):
	Attenuation_Factor_DEFAULT=21000.0; 
	I_Volts_background_DEFAULT = 0.1;  
	Attenuation_Factor=Attenuation_Factor_DEFAULT; // in cm^2

	//sigmAtt=.1;  //std of the attenuation (including variation of reflectivity). This quantity must be adjusted once at calibration, by measuring it at a known distance
	//I_mW_background = 0.0000173163;// background noise intensity, in mW
	//sigm_I_mW_background = 0.000001;  // background noise intensity std deviation, in mW. 

	// **** The following parameters should be pre-calibrated (finishing putting them toghether here - TO DO!)
        depth_Range_REF=80; // reference distance is depth_Range_REF (in cm) from the system (this is the reference distance corresponding to a circular saccade R_cm_REF).
	R_cm_REF=1;	// this is the REFERENCE RADIUS of the saccade (in cm), for a saccade depth_Range_REF centimeters away from the system. 
       //RedLaserPower_REF; //reference laser power in mW, calculated in RangeCalibration. 
	//Rem: conversion bewtween volts and watts is very simple for the 1mW premium laser diode: 1V=1mW. 
         //--------------------------------------------------------- -----------------------------------------------------  -----------------------------------------------------  

         // DEFAULT VALUES (as if a measure has been taken - this is useful for starting the corrections, otherwise we need to check if it's the first time...): 
        I_Volts_REF=3.73; // laser power corresponding to the optimal value of intensity
        I_Volts_background=I_Volts_background_DEFAULT;
        I_Volts= I_Volts_REF;
        MIN_CONTRAST_Volts=.45; // minimal variation of the intensity required to properly distinguish a light and a dark zone in the saccade, and then compute the recentering vector (delta_Az, delta_El)

	// **** ZONE DELIMITERS (for *this* spot):
	// REM: the following is not so nice, it would be better to use the hardwareIO class:
	xMax = 3800; xMin = 200;
	yMax = 3800; yMin = 200;

	// **** Depth_range delimiter (a computed range farest than that means NO hit, and the angular correction does not work)
	MANUAL_MAX_DEPTH_RANGE=500; // in cm
	MANUAL_MIN_DEPTH_RANGE=10; // in cm (to avoid problems with the optical system).
	CALIBRATED_MAX_DEPTH_RANGE=MANUAL_MAX_DEPTH_RANGE;
	CALIBRATED_MIN_DEPTH_RANGE=MANUAL_MIN_DEPTH_RANGE;	
	// the absolute maximum and minimum ranges that "make sense":
	MAX_DEPTH_RANGE=min(MANUAL_MAX_DEPTH_RANGE, CALIBRATED_MAX_DEPTH_RANGE);
	MIN_DEPTH_RANGE=max(MANUAL_MIN_DEPTH_RANGE, CALIBRATED_MIN_DEPTH_RANGE);
        
        // IMPORTANT: we need to initialize depth_Range, even if it's bad, so we can do proper computation of laser correction, etc. 
       depth_Range=  depth_Range_REF; // choose the value that was used for the calibration corresponding to the stored parameters. 
        
	// Angular correction, in radians
#ifdef NO_HARDWARE
	error_angle = 0;
#else
	error_angle = -0.279;// -.433;
#endif
	deltaAz=0; deltaEl=0; 

	// **** Preprocessing saccade modes: 
        filtering_Mode=NO_FILTER;
	thresholding_Mode =FIXED_THRESHOLD; //AUTO_THRESHOLD, DEPTH_THRESHOLD
	FIXED_THRESHOLD_FACTOR=0.6;//if this is 0.5, it means that the fixed_Threshold_Volts  is set to HALF the optimal contrast (I_Volts_REF-I_Volts_background)
	AUTO_THRESHOLD_FACTOR=0.6;//2.0/3; // (0 to 1), controls the setting of the automatic threshold: auto_Threshold_Volts=(maxI_Volts-minI_Volts)*AUTO_THRESHOLD_FACTOR+minI_Volts (1/2 or 2/3 works fine)
	// The actual threshold variables: 
	auto_Threshold_Volts=1.5;
        fixed_Threshold_Volts=1.5;// REM: it can be set by hand, or can be set after launching a calibration process. 
	depthThreshold=150; //in cm. This is for contour following on contour lines (instead of black/white objects, in this case we would use autoThreshold). Could be changed using potentiometer??

	//Minimum contrast factor to discriminate zones:
	MIN_CONTRAST_FACTOR=.25; // (.25 ok)  a good value should be something like .1 (a tenth of the maximum contrast available...)
	binary_smoothing_Mode=NO_MORPHO_SMOOTH; // this means that the data is not "smoothed" before "light-zones" computation.
	whiteProportion=0; // this is the proportion of samples in the white zone in the saccade
	// flag_sens=1; ????

	// **** RED laser: 
	RedLaserPower=0.5*MAX_RedLaserPower; //initial laser power in mW
	//inter_Saccade_RedLaserPower=0.0; 
	//MAX_RedLaserPower=3; MIN_RedLaserPower=0; // these are now in hardwareIO 
	IO.setRedPower_mW(RedLaserPower);

        // *** Green laser:
        GreenLaserPower=LOW;
        IO.setGreenPower(GreenLaserPower);

        // **** Laser power adjustement: 
	LaserCorrectionDamping=.6; 
	PowerCorrectionMode= POWER_ADJUST; // POWER_FIXED

        // **** Radius Adjustement Mode and parameters: 
	RadiusCorrectionMode= RADIUS_FIXED;// RADIUS_ADJUST; // start with RADIUS_FIXED, as the depth can be bad because we didn't do calibration 
	RadiusCorrectionDamping=.6; // the factor is a damping factor to avoid oscillations.
	
	// **** CALIBRATION: 
	min_delta_RedLaserPower=5.0/MAX_AD_RED; // minimum correction for the laser power (used in the gradient descend stop test in the "optimal" calibration phase routine, as well as in the
        // treatement of cases where we cannot update the laser power by calculating the linear relation between measured intensity and laser power (i.e., in the case that the measurement just correspond
        // to the background noise... ).
        // Note: the resolution of the SPI ADC is 10 bits (values between 0 and 4096), for a max voltage of around 5V: 5.0/MAX_AD_RED=0.00122. But it seems that
       // the laser output does not change after an AD value of around 2000 (2.4V).  
	RedLaserPower_REF=1.0; // in mW, actually, this is a variable only used by the CalibrationRange routine, and we don't need to save it. 
	I_Volts_OPTIMAL_REF_FACTOR=.5; // the working value of the photodetector intensity (with respect to the saturation of the PD)
        // REM: 50% should be optimal, as long as we are far from the noise (this could be computed too...)
	I_Volts_UpperMarge_REF_FACTOR=.9;
	I_Volts_LowerMarge_REF_FACTOR=.2;
        I_Volts_OPTIMAL= I_Volts_OPTIMAL_REF_FACTOR*I_Volts_SAT; // REM: I_Volts_SAT is set in the hardwareIO file
	// per-sample depth computation mode
	computeDepthPerSample=false; 


	// Newtonian parameters: 
	firstTime=true; // this is used to set the x(t-dt) for verdet integration the first time we update the position.
	vx=0; vy=0; initVx=0; initVy=0;
	ax=0; ay=0; 
	//updatePosition(posX, posY); // this will also set the value of touchWall
	// rem: the value of collision for each spot is calculated outside the class, but must be initialized here
	collision=false;
	// rem: angles of lines calculated in computeNormalVector
	target_angle=0;// the direction of the recentering vector

	// when in newtonian dynamics:
	bouncingForceMode=true; 
	bouncingSpecularMode=false;
	centralForceMode=false; // default mode is CENTRAL force active
	parallelForceMode=false; // default mode is parallel force inactive
	particleInteractionMode=true;// default mode 
	visForceMode=false; // default mode false	
	bouncingForceFactor=50;// needs to be calibrated
	centralForceFactor=0.0003; // this is for a SPRING like force! (not gravitational)
	// parallel accelearion: (REM: we could use an accelerometer to control this!!!)
	gx=0;  gy=-1;
	//interForceFactor=10;//7.5;// for a force on 1/r
	interForceFactor=3000; // for a force on 1/r^2
	// interForceFactor=0.00000002; // for a force that is spring-like
	visForceFactor=0.001; // fraction of the speed lost to fraction (unit-less), for calculating the viscosity force
	totalForceX=0; totalForceY=0; // acceleration is calculated form this, divided by the mass of the particle
	interForceX=0; interForceY=0; // the force given by interaction with other particles or objects (gravity+electrostatic)
	rangeInteractionLong=Aper_DA*80; // must be calibrated!! (potentiometer?)
	rangeInteractionShort=Aper_DA*5; // must be calibrated!! (potentiometer?)
	mass=0.05;//pmass;   // for calculating the gravitational force, as well as the acceleration
	charge=1;//pcharge; // for calculating electrostatic force
	dt=5; // my initial measures gave dt=5ms
	oldTime=micros();
	slowTime=1.0/15; // (the effective dt will be dt*slowTime)

	// Default parameters for each BEHAVIOUR MODE (make initializators for EACH mode - i.e., in the future we will use a subclass): 
	// (1) Simulated refraction:
	firstTimeStartRefraction=true; // this is for the snell law mode
        countsign=1;
	// (2) contour following: 
	onContour=false; // start as if there was no contour (activate search)
	attemptsFollowing_counter=0; maxAttemptsFollowing=2;
        attemptsSearching_counter=0; maxAttemptsSearching=15; 

	_idAv = 0;
	thresholdFactorFB = 1.3;
	speedContourFollowing = 0.9;//0.85; // controls the speed of contour following (proportion of radius)
	
	// collision detection (meaning that another particle is in nearby, inside collisionRadius)
	collisionRadius=Aper_DA*2; // must be calibrated!
	collision=false;

	// border mode:
	borderDamping=true;
	borderMode=NORMAL_REFLEXION;//RESET_POSITION;// RETRO_REFLEXION;
	showLimits=true;
	showGreenFlash=true;

        // Miscelanea common things:
        traceNormalVector=false;
        displayDistance=false;
        displayAngle=false;
        displayMonocycle=false;
}

//LivingSpot::LivingSpot(int threshold, int posX, int posY, int saccadeRadius, int numSample, int ident) {
//    Init(threshold, posX, posY, saccadeRadius, numSample, ident);
//}


// ===================================================================================================

// The main update function: 
void LivingSpot::update(LivingSpot* spotArray, int numSpots) {
	if (alive==true) { // else do nothing
		switch (behaviourMode) {
		case STAND_CENTER:
                        standBy();
			break;
                case DEPTH_LETTERS:
                       showLetters(); // same that computeNormalVector, but without the additional points for the circular saccade
                break;
                case TRACKING:
                      tracking();
                break;
		case SIMPLE_BOUNCING:
			collision = simpleBouncing(numSpots);
			break;
                case SIMPLE_BOUNCING_CENTRAL:
                        collision = simpleBouncingCentralForce(numSpots);
                        break;
		case FAST_BOUNCING:
			collision = fastBouncing();
			break;
		case NEWTON_DYNAMICS:
			// first, compute inter-particle forces if necessary:
			if (particleInteractionMode) {
				//for (int i=0; i<numSpots; i++) {spotArray[i].interForceX=0; spotArray[i].interForceY=0;};
				for (int i=0; i<numSpots; i++) {
					spotArray[i].collision=spotArray[i].computeInteraction(spotArray, numSpots); // this will set forceInteractionX and Y for "this" spot, as well as others in the spotArray list. It also computes the present value of "collision"
				}
			}
			// then, do Euler or Verlet integration:
			newtonDynamics(numSpots);
			break;
		case COUNTOUR_FOLLOWING:
                        // BEHAVIOUR (including search): 
			if (onContour) onContour=moveAlongContour(numSpots); // while we are on the contour, continue following
                        //if (true) onContour=moveAlongContour(numSpots); // while we are on the contour, continue following
			else { // contour tracking is lost
				if (attemptsFollowing_counter<maxAttemptsFollowing) { // continue with the routing for a while, perhaps it will catch the contour again
					attemptsFollowing_counter++;
					if(onContour=moveAlongContour(numSpots)) attemptsFollowing_counter=0;
				} else { // start searching for something: 
					attemptsFollowing_counter=0; // reset attemptsFollowing_counter for the next time it starts re-tracking
					attemptsSearching_counter++;
					onContour=searchContour(numSpots);
					if (attemptsSearching_counter>=maxAttemptsSearching) { // this means nothing here: move the spot randomly or recenter it
						moveSpot(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y);//random(CENTER_AD_MIRROR_X), random(CENTER_AD_MIRROR_X));
						attemptsSearching_counter=0;
					}  
				}
			}
			// reverse direction of motion when touching a wall:
			if (touchWall) speedContourFollowing*=-1;
			break;

		case SNELL_LAW:
			lightRefraction();
			break;
		default:
			break;
		}
                
                // DISPLAY RECENTERING VECTOR:
                if ( traceNormalVector ) { // decoration: trace the normal vector in green
                 IO.setGreenPower(HIGH);
                 for (int i=0; i<4;i++) {
			moveUncheckedSpot(Az, El); 
                        delayMicroseconds(300); 
                        moveUncheckedSpot( Az+deltaAz,  El+deltaEl);
                        //moveUncheckedSpot( Az+10*Aper_DA*nx,  El+10*Aper_DA*ny);
                         delayMicroseconds(300);
		 }
                 IO.setGreenPower(LOW);
                }
                
		// Common action for all modes:
		if (collision) {
                        // VISUALS: 
			if (showGreenFlash) {
				//spotArray[0].GreenLaserPower=HIGH;
                                GreenLaserPower=HIGH;
				computeNormalVector(Az, El, 1.5*Aper_DA);
                                computeNormalVector(Az, El, 1.5*Aper_DA);
				computeNormalVector(Az, El, 1.5*Aper_DA);
				//spotArray[0].GreenLaserPower=LOW;
                                GreenLaserPower=LOW;
			}
                       // REM: sound is treated in the main program through the function midiSoundGeneration()
		}

		if (touchWall) {     
			
                         // VISUALS: 
                         // show contour:
			if (showLimits) {
                          /*
                                // square: 
                                IO.setGreenPower(HIGH);
                                IO.setRedPower_mW(MIN_RedLaserPower);
                                drawMaxArea(2);
                                IO.setGreenPower(LOW);
                                // or circle: 
                                spotArray[0].GreenLaserPower=LOW;
                                spotArray[0].updatePosition(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y);
                                spotArray[0].GreenLaserPower=HIGH;
                                spotArray[0].computeNormalVector(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, i);
                                spotArray[0].computeNormalVector(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, i);
                                spotArray[0].GreenLaserPower=LOW;
                          */    
			}
		}        
	}
}

// ===================================================================================================

// Update the position variables (Az, El) with test on the border constraints (in fact, ANY scanning/drawing trajectory should be inside
// a square of dimensions 2*Aper_ADx2*Aper_DA (for the time being, this is useful for a circular saccade). 
int LivingSpot::updatePosition(float posX, float posY) {
	int returnedValue = 1;
	// if everything is ok, then we would have these values: 
	Az = posX; El = posY; // rem: Az and El are both floats (only converted to int when actually positioning the mirrors)

	// limit control: 
	switch(borderMode) {// border mode (int borderMode):
	  case NORMAL_REFLEXION:
		  // normal reflexion:
		  if (behaviourMode!=NEWTON_DYNAMICS) {
			  if(Az > xMax - Aper_DA){
				  Az = xMax - Aper_DA;
				  if (borderDamping) vx = -vx*0.98; else  vx = -vx;
				  returnedValue = 0;
			  }
			  if(El > yMax - Aper_DA){
				  El = yMax - Aper_DA;
				  if (borderDamping) vy = -vy*0.98; else  vy = -vy;
				  returnedValue = 0;
			  }
			  if(Az < xMin + Aper_DA){
				  Az = xMin + Aper_DA;
				  if (borderDamping) vx = -vx*0.98; else  vx = -vx;
				  returnedValue = 0;
			  }
			  if(El < yMin + Aper_DA){
				  El = yMin + Aper_DA;
				  if (borderDamping) vy = -vy*0.98; else  vy = -vy;
				  returnedValue = 0;
			  }
		  } else { // this is in the case of VERLET integration:
			  if (posX > xMax - Aper_DA) { //||(posX < xMin + Aper_DA))
				  if (borderDamping) {Az_old=0.85*(xMax - Aper_DA)+0.15*Az; // this will effectively invert the direction (Az_old=posX would do, but I want some damping...)
				  // and we don't change x(t) (i.e. the variable Az)
				  } else Az_old= xMax - Aper_DA;
				  returnedValue = 0;  
			  } else Az=posX;

			  if (posX < xMin + Aper_DA) {
				  if (borderDamping) {Az_old=0.85*(xMin + Aper_DA)+0.15*Az; // this will effectively invert the direction (Az_old=posX would do, but I want some damping...)
				  // and we don't change x(t) (i.e. the variable Az)
				  } else Az_old= xMin + Aper_DA;
				  returnedValue = 0;  
			  } else Az=posX;

			  if (posY > yMax - Aper_DA) {
				  if (borderDamping) {El_old=0.85*( yMax - Aper_DA)+0.15*El; // this will effectively invert the direction (Az_old=posX would do, but I want some damping...)
				  } else El_old=( yMax - Aper_DA);
				  returnedValue = 0;  
			  } else El=posY;

			  if (posY < yMin + Aper_DA) {
				  if (borderDamping) {El_old=0.85*(yMin + Aper_DA)+0.15*El; // this will effectively invert the direction (Az_old=posX would do, but I want some damping...)
				  } else El_old=(yMin + Aper_DA);
				  returnedValue = 0;  
			  } else El=posY;

		  }
		  break;
	  case RETRO_REFLEXION: // this is a special reflection in which the speed is just inverted when the border is touched:
		  if (behaviourMode!=NEWTON_DYNAMICS) {
			  if ((posX > xMax - Aper_DA)||(posY > yMax - Aper_DA)||(posX < xMin + Aper_DA)||(posY < yMin + Aper_DA)) {
				  if (borderDamping) {
					  vx=-vx*0.98; 
					  vy=-vy*0.98;
				  } else {
					  vx=-vx; 
					  vy=-vy;
				  }
				  if (posX > xMax - Aper_DA) Az = xMax - Aper_DA;
				  if (posY > yMax - Aper_DA) El = yMax - Aper_DA;
				  if (posX < xMin + Aper_DA) Az = xMin + Aper_DA;
				  if (posY < yMin + Aper_DA) El = yMin + Aper_DA;

				  returnedValue = 0;
			  }
		  } else { // case of VERLET integration:
			  if ((posX > xMax - Aper_DA)||(posY > yMax - Aper_DA)||(posX < xMin + Aper_DA)||(posY < yMin + Aper_DA)) {
				  float auxX=Az; Az=Az_old; Az_old=auxX;
				  float auxY=El; El=El_old; El_old=auxY;
				  returnedValue = 0;
			  }
		  }
		  break;
	  case RESET_POSITION: 
		  if ((posX > xMax - Aper_DA)||(posY > yMax - Aper_DA)||(posX < xMin + Aper_DA)||(posY < yMin + Aper_DA)) {
			  Az=Az_Default; El=El_Default;
			  vx=initVx; vy=initVy;
			  firstTime=true; // to set Az_old and El_old in case of VERLET integration
			  firstTimeStartRefraction=true; // this is for the snell law mode
			  // In fact, here we should call all the "reset" methods of all the behaviour modes...
			  returnedValue = 0;
		  }
	  default:
		  break; 
	}

	if (returnedValue==0) touchWall=true;
	else touchWall=false;

	// new: in case of newtonian dynamics, because of more precise verdet integration, we need to set for the FIRST time the x(t-dt):
	if (firstTime) {
		firstTime=false;
		Az_old=Az_Default;
		El_old=El_Default;

		// stop green and red laser to avoid a line between last position and new start position:
		//IO.setRedPower_mW(0);//Spi.writeOutR(0);
		IO.setGreenPower(LOW);
		moveSpot(Az, El); // we can use moveSpot (the laser will position itself in the center of the saccade)
		delayMicroseconds(200);
	}

	return returnedValue; // return 1 if spot does not touch wall (or when we reset the position), and 0 when it does touch limits
}

// ===================================================================================================

//Actually MOVE the spot to the stored position, but with border check (it calls updatePosition): 
// REM: the version without border check is just IO.setMirrorsXY_AD.
int LivingSpot::moveSpot(float posX, float posY){ // rem: parameters are float, because updatePosition uses also floats (in fact, because Az and El are floats)
	int returnedValue = updatePosition(posX, posY); // this will update the current position (Az, El) with proper border clipping. 
	IO.setMirrorsXY_AD(int(Az), int(El)); // Az and El are floats - only when setting the mirrors we go with integers. 
	// (otherwise, don't change Az and El). 
	return returnedValue; // return 0 when spot touched limits, 1 if nothing happened. 
}

void LivingSpot::moveUncheckedSpot(float posX, float posY){ 
	IO.setMirrorsXY_AD(int(posX), int(posY)); // Az and El are floats - only when setting the mirrors we go with integers. 
}

// ===================================================================================================

// get Az and El (if set as private variables)
float LivingSpot::getAz(){return Az;}
float LivingSpot::getEl(){return El;}

// ===================================================================================================

// --------------THRESHOLD UPDATE -------------------------------------------------------------------------

// Compute the auto-threshold value, given two measures of signal (max and min, in particular in a scanning saccade).
// Note: returns the autothreshold, or the min reading if there is not enough contrast (MIN_CONTRAST_Volts can be calculated 
// once and for all in RangeCalibration (it is either manual or equal to 2xsigmI_AD for instance), or when we call analyzeScanData (using the
// measured standard deviation of the signal). 
float LivingSpot::computeAutoThreshold(float maxVoltsMeasured, float minVoltsMeasured) {
	float thresh;	
	// Minimum intensity variation MIN_CONTRAST_Volts needed in order to set the automatic threshold:
	if ((maxVoltsMeasured-minVoltsMeasured) > MIN_CONTRAST_Volts ) { 

		thresh=1.0*(maxVoltsMeasured-minVoltsMeasured)*AUTO_THRESHOLD_FACTOR+minVoltsMeasured;
	}
	else {// ... otherwise, we consider that the saccade is FULL on the object, by doing:
		thresh=minVoltsMeasured;
	}
	return(thresh);
}

// overloaded function using the max and min on the last computed saccade: 
float LivingSpot::computeAutoThreshold() {
	float thresh;	
	// Minimum intensity variation MIN_CONTRAST_Volts needed in order to set the automatic threshold:
	if (Proper_Contrast_Flag ==GOOD_CONTRAST) { 
		thresh=1.0*(maxI_Volts-minI_Volts)*AUTO_THRESHOLD_FACTOR+minI_Volts;
	}
	else {// ... otherwise, we consider that the saccade is FULL on the object, by doing:
		thresh=minI_Volts;
              // (in fact, in the analyzeScanData method, we can check for good contrast BEFORE going into the trouble of thresholing the saccade...) 
	}
	return(thresh); 
}



// ===================================================================================================

//--------------- RANGE COMPUTATION -----------------------------------------------------------------------
// Computation of the range as a function of the voltage readout (will work after calibration), without depth limits control, but with control on the intensity, i.e.
// good or bad intensity flag (otherwise, we can have a negative square root when I_volts < I_volts background - something that can happens if we cover the photodetector for instance...)
//rem: range could be computed even though the intensity is lower than I_mW_LowerMarge or 
//larger than I_mW_UpperMarge (values won't be very good though, and even can lead to +/-INF)!!!!!
float LivingSpot::ComputeRangeUnconstrained(double paramI_Volts, double paramRedLaserPower) {
	float range=0;
	
          if (Proper_Intensity_Flag==SATURATED_SIGNAL) range=MAX_DEPTH_RANGE;
         else if (Proper_Intensity_Flag==TOO_WEAK) range=MIN_DEPTH_RANGE;
         else // i.e. when Proper_Intensity_Flag=APPROPRIATE_STRENGH, do the actual calculation:
         { 

         IO.getAnglesFromAD(Az_Degree, El_Degree, Az, El); // will modify AzDegree and ElDegree
	//AzDeg=AzAD*AD_to_Deg_MIRROR_X+MIN_DEG_MIRROR_X;
	//ElDeg=ElAD*AD_to_Deg_MIRROR_Y+MIN_DEG_MIRROR_Y;

	// local variables: 
	float Az_Rad=Az_Degree*PI/180;
	float El_Rad=El_Degree*PI/180;

	float Q,R,D, p,q;
	//complex<float> S,T, z1, z2, z3, im(0,1);
	float b,c, x1, x2;

	// SEVERAL MODELS: 
	switch(Distance_Intensity_Model) {
		case PRE_CALIBRATED_SKIN: // in fact, this pre-calibration of parameters should be independent of the model, 
			// but it is assumed that this precalibration implies the SIMPLISTIC model (then, no break here).
			// use pre-calibrated model (26.2.2005), on skin (hand):
			//break;
		case  ORDER_0: 	
			// ... SIMPLISTIC MODEL, which is just an order 0 developpement of the exact intensity-distance formula: 
			range= 1.0*sqrt(Attenuation_Factor*cos(Az_Rad)*cos(El_Rad)*paramRedLaserPower/(paramI_Volts-I_Volts_background));
			// AD-HOC corrections on the model:
			// factor_Adjust
			// range_Adjust is public, so we can "tune" the model in the main program.
			break;
		case  ORDER_0_CORRECTED_LENS: 	// uses exp_Cos exponent for the cosinus
			// ... most SIMPLISTIC MODEL, which is just an order 0 developpement of the exact intensity-distance,
			// with NO COSINUS relation for the photodetector: 
			range= 1.0*sqrt(Attenuation_Factor*pow(cos(Az_Rad)*cos(El_Rad),exp_Cos)*paramRedLaserPower/(paramI_Volts-I_Volts_background));
			// AD-HOC corrections on the model:
			// factor_Adjust
			// range_Adjust is public, so we can "tune" the model in the main program.
			break;
		case ORDER_1: // not used for the time being (in particular, I don't have the "complex" type in the arduino
			/*
			// The really meaninful, physically based model is the "standard" model including the Lambertian "cos" relation between intensity and voltage on the photodetector.
			// This model also includes the separation between photodetector and mirrors (value PD_X and PD_Y), and the first order 
			// developpement when PD_X and PD_Y are SMALL compared to depth_Range implies solving a cubic equation in the 
			// standard form (see MathWorld), in order to get depth_Range:
			p=-1.0*Attenuation_Factor*paramRedLaserPower/(paramI_Volts-I_Volts_background)*cos(Az_Rad)*cos(El_Rad);
			q=p*(3*PD_Y*sin(El_Rad)-3*PD_X*cos(El_Rad)*sin(Az_Rad));
			Q=p/3;
			R=q/2;
			D=pow(Q,3)+pow(R,2); 
			if (D<0) S=R+im*sqrt(-D); else S=R+sqrt(D);
			S=pow(S, 1.0/3);
			if (D<0) T=R-im*sqrt(-D); else T=R-sqrt(D);
			T=pow(T, 1.0/3);
			//REM: determining which roots are real and which are complex can be 
			//accomplished by noting that if the polynomial discriminant D > 0, one root is 
			//real and two are complex conjugates; if D = 0, all roots are real and at least 
			//two are equal; and if D < 0, all roots are real and unequal. 
			// Then, the solutions are:
			z1=S+T;
			z2=-(S+T)*.5+(S-T)*.5*im*(double)sqrt((double)3);
			z3=-(S+T)*.5-(S-T)*.5*im*(double)sqrt((double)3);
			// we must choose the (unique) real, positive solution:
			range=-1;
			if ((imag(z1)==0)&&(real(z1)>=0)) range=real(z1);
			if ((imag(z2)==0)&&(real(z2)>=0)&&(real(z2)>range)) range=real(z2);
			if ((imag(z3)==0)&&(real(z3)>=0)&&(real(z2)>range)) range=real(z3);
			// ----------------------------------------------------------------------------------------------------
			*/
			break;
		case ORDER_1_CORRECTED_LENS:
			// A AD-HOC modified standard model, which supposes that instead of a "cos" Lambertian relation on the photodetector 
			// surface, we have a cos^2 relation (because of the lens in front of the photodetector). The solution for 
			// depth_Range can be computed explicitly, by just solving a SECOND DEGREE POLYNOMIAL EQUATION:
			b=-(2*cos(El_Rad)*sin(Az_Rad)*PD_X+2*sin(El_Rad)*PD_Y+cos(El_Rad)*cos(Az_Rad)*sqrt(Attenuation_Factor*paramRedLaserPower/(paramI_Volts-I_Volts_background)));
			//std::cout << " Laser : " << paramRedLaserPower << "   b: " << b << endl;
			c=PD_X*PD_X+PD_Y*PD_Y;
			D=b*b-4*c; // the discriminant... Because this is a PHYSICAL-based model, D should be D>=0... Moreover, a
			// unique positive root should be found. Anyway, just in case something goes weird on the computation (or in the physical world ;) we test it:
			if (D>=0) {
				x1=(-b+sqrt(D))/2;
				x2=(-b-sqrt(D))/2;
				// the range is the larger positive root (normally, only one positive root should be found, right?):
				range=-1;
				if (x1>=0) range=x1;
				if ((x2>=0)&&(x2>range)) range=x2;
			}
			else {
				//std::cout << " There is a problem, discriminant of second order equation is <0 " << endl; 
			}
			break;
		case ORDER_1_NO_COSINUS:// this is a model which does not include a cos relation for the photodetector. The model is otherwise "exact" and
			// depth_Range can be exactly resolved as in ORDER_1_CORRECTED_LENS, by solving a second degree equation.
			b=-2.0*(PD_X*cos(El_Rad)*sin(Az_Rad)+sin(El_Rad)*PD_Y);
			//std::cout << " Laser : " << paramRedLaserPower << "   b: " << b << endl;
			c=PD_X*PD_X+PD_Y*PD_Y-Attenuation_Factor*paramRedLaserPower/(paramI_Volts-I_Volts_background);
			D=b*b-4*c; // the discriminant... Because this is a PHYSICAL-based model, D should be D>=0... Moreover, a
			// unique positive root should be found. Anyway, just in case something goes weird on the computation (or in the physical world ;) we test it:
			if (D>=0) {
				x1=(-b+sqrt(D))/2;
				x2=(-b-sqrt(D))/2;
				// the range is the larger positive root (normally, only one positive root should be found, right?):
				range=-1;
				if (x1>=0) range=x1;
				if ((x2>=0)&&(x2>range)) range=x2;
			}
			else {
				//std::cout << " There is a problem, discriminant of second order equation is <0 " << endl; 
			}
			break;
		default:
			break;
	}
         }
         
     // Check the results: 
    if (range>=MAX_DEPTH_RANGE) Proper_Distance_Flag=TOO_FAR;
    else if (range<=MIN_DEPTH_RANGE) Proper_Distance_Flag=TOO_CLOSE;
    else  Proper_Distance_Flag=PROPER_RANGE;

    return(range); 
}


// Output is distance in cm, with proper range clipping: 
float LivingSpot::ComputeRange(float paramI_Volts, float paramRedLaserPower) {
      float range;
      
     range=ComputeRangeUnconstrained(I_Volts, RedLaserPower);// this computes the depth from the tracking system to the target, with PROPER INTENSITY CHECK
      
      // clipping: 
      if ( Proper_Distance_Flag==TOO_FAR) range=MAX_DEPTH_RANGE;
      else if ( Proper_Distance_Flag==TOO_CLOSE) range=MIN_DEPTH_RANGE;

     return(range);
}

// depth range to volts (useful to compute the depthThreshold_Volts: 
float LivingSpot::computeIntensityVolts(float range, float paramRedLaserPower) {
	// REM: we need to use the SAME model used to compute the range as a function of the intensity (otherwise the measured distance and the threshold won't match):
	float computed_I_Volts=0;	

        IO.getAnglesFromAD(Az_Degree, El_Degree, Az, El); // will modify AzDegree and ElDegree
	//AzDeg=AzAD*AD_to_Deg_MIRROR_X+MIN_DEG_MIRROR_X;
	//ElDeg=ElAD*AD_to_Deg_MIRROR_Y+MIN_DEG_MIRROR_Y;

        // local variables: 
	float Az_Rad=Az_Degree*PI/180;
	float El_Rad=El_Degree*PI/180;

	switch(Distance_Intensity_Model) {
		case PRE_CALIBRATED_SKIN: // in fact, this pre-calibration of parameters should be independent of the model, 
			// but it is assumed that this precalibration implies the SIMPLISTIC model (then, no break here).
			// use pre-calibrated model (26.2.2005), on skin (hand):
			//break;
		case  ORDER_0: 	
			// ... SIMPLISTIC MODEL, which is just an order 0 developpement of the exact intensity-distance formula: 
			//range= 1.0*sqrt(Attenuation_Factor*cos(Az_Rad)*cos(El_Rad)*paramRedLaserPower/(paramI_Volts-I_Volts_background));
			computed_I_Volts=Attenuation_Factor*cos(Az_Rad)*cos(El_Rad)*paramRedLaserPower/(range*range)+I_Volts_background;
			break;
		case  ORDER_0_CORRECTED_LENS: 	// uses exp_Cos exponent for the cosinus
			// ... most SIMPLISTIC MODEL, which is just an order 0 developpement of the exact intensity-distance,
			// with NO COSINUS relation for the photodetector: 
			//range= 1.0*sqrt(Attenuation_Factor*pow(cos(Az_Rad)*cos(El_Rad),exp_Cos)*paramRedLaserPower/(paramI_Volts-I_Volts_background));
			computed_I_Volts=Attenuation_Factor*pow(cos(Az_Rad)*cos(El_Rad),exp_Cos)*paramRedLaserPower/(range*range)+I_Volts_background;
			break;
		case ORDER_1: // not used for the time being (in particular, I don't have the "complex" type in the arduino
			/*
			// The really meaninful, physically based model is the "standard" model including the Lambertian "cos" relation between intensity and voltage on the photodetector.
			// This model also includes the separation between photodetector and mirrors (value PD_X and PD_Y), and the first order 
			// developpement when PD_X and PD_Y are SMALL compared to depth_Range implies solving a cubic equation in the 
			// standard form (see MathWorld), in order to get depth_Range:
			p=-1.0*Attenuation_Factor*paramRedLaserPower/(paramI_Volts-I_Volts_background)*cos(Az_Rad)*cos(El_Rad);
			q=p*(3*PD_Y*sin(El_Rad)-3*PD_X*cos(El_Rad)*sin(Az_Rad));
			Q=p/3;
			R=q/2;
			D=pow(Q,3)+pow(R,2); 
			if (D<0) S=R+im*sqrt(-D); else S=R+sqrt(D);
			S=pow(S, 1.0/3);
			if (D<0) T=R-im*sqrt(-D); else T=R-sqrt(D);
			T=pow(T, 1.0/3);
			//REM: determining which roots are real and which are complex can be 
			//accomplished by noting that if the polynomial discriminant D > 0, one root is 
			//real and two are complex conjugates; if D = 0, all roots are real and at least 
			//two are equal; and if D < 0, all roots are real and unequal. 
			// Then, the solutions are:
			z1=S+T;
			z2=-(S+T)*.5+(S-T)*.5*im*(double)sqrt((double)3);
			z3=-(S+T)*.5-(S-T)*.5*im*(double)sqrt((double)3);
			// we must choose the (unique) real, positive solution:
			range=-1;
			if ((imag(z1)==0)&&(real(z1)>=0)) range=real(z1);
			if ((imag(z2)==0)&&(real(z2)>=0)&&(real(z2)>range)) range=real(z2);
			if ((imag(z3)==0)&&(real(z3)>=0)&&(real(z2)>range)) range=real(z3);
			// ----------------------------------------------------------------------------------------------------
			*/
			break;
		case ORDER_1_CORRECTED_LENS: // 17.7.2010 CHECK THIS!!!
			// A AD-HOC modified standard model, which supposes that instead of a "cos" Lambertian relation on the photodetector 
			// surface, we have a cos^2 relation (because of the lens in front of the photodetector). 
			computed_I_Volts=I_Volts_background+Attenuation_Factor*paramRedLaserPower*range*cos(El_Rad)*cos(Az_Rad);
			computed_I_Volts/=pow(sq(range*cos(Az_Rad)*sin(El_Rad)-PD_X)+sq(range*sin(Az_Rad)-PD_Y)+sq(range*cos(Az_Rad)*cos(El_Rad)),3.0/2);
			computed_I_Volts+=I_Volts_background;

			/*
			//The solution for depth_Range can be computed explicitly, by just solving a SECOND DEGREE POLYNOMIAL EQUATION:
			b=-(2*cos(El_Rad)*sin(Az_Rad)*PD_X+2*sin(El_Rad)*PD_Y+cos(El_Rad)*cos(Az_Rad)*sqrt(Attenuation_Factor*paramRedLaserPower/(paramI_Volts-I_Volts_background)));
			//std::cout << " Laser : " << paramRedLaserPower << "   b: " << b << endl;
			c=PD_X*PD_X+PD_Y*PD_Y;
			D=b*b-4*c; // the discriminant... Because this is a PHYSICAL-based model, D should be D>=0... Moreover, a
			// unique positive root should be found. Anyway, just in case something goes weird on the computation (or in the physical world ;) we test it:
			if (D>=0) {
			x1=(-b+sqrt(D))/2;
			x2=(-b-sqrt(D))/2;
			// the range is the larger positive root (normally, only one positive root should be found, right?):
			range=-1;
			if (x1>=0) range=x1;
			if ((x2>=0)&&(x2>range)) range=x2;
			}
			else {
			//std::cout << " There is a problem, discriminant of second order equation is <0 " << endl; 
			}
			*/
			break;
		case ORDER_1_NO_COSINUS:// this is a model which does not include a cos relation for the photodetector. The model is otherwise "exact" and
			/*
			// depth_Range can be exactly resolved as in ORDER_1_CORRECTED_LENS, by solving a second degree equation.
			b=-2.0*(PD_X*cos(El_Rad)*sin(Az_Rad)+sin(El_Rad)*PD_Y);
			//std::cout << " Laser : " << paramRedLaserPower << "   b: " << b << endl;
			c=PD_X*PD_X+PD_Y*PD_Y-Attenuation_Factor*paramRedLaserPower/(paramI_Volts-I_Volts_background);
			D=b*b-4*c; // the discriminant... Because this is a PHYSICAL-based model, D should be D>=0... Moreover, a
			// unique positive root should be found. Anyway, just in case something goes weird on the computation (or in the physical world ;) we test it:
			if (D>=0) {
			x1=(-b+sqrt(D))/2;
			x2=(-b-sqrt(D))/2;
			// the range is the larger positive root (normally, only one positive root should be found, right?):
			range=-1;
			if (x1>=0) range=x1;
			if ((x2>=0)&&(x2>range)) range=x2;
			}
			else {
			//std::cout << " There is a problem, discriminant of second order equation is <0 " << endl; 
			}
			*/
			break;
		default:
			break;
	} 
	return(computed_I_Volts);
}

// ===================================================================================================

// NEW (17.7.2010): compute per-sample range (using laser power updated per-saccade basis: this is a preliminary solution that may work if the saccade does not have wild variations on its intensity):
// When computeDepthPerSample==true, the depthSaccade[] array will be filled in the computeDepthSamples (using the _rawScanData[] values, and the "global-saccade" laser power, delta_RedLaserPower+RedLaserPower
void LivingSpot::computeDepthSamples(float saccade_redlaserpower, int numsamples) {
	for (int i=0; i<numsamples; i++) depthSaccade[i]=ComputeRange(_rawScanData[i], saccade_redlaserpower); // compute range with clipping. 
}

// Instead of thresholding directly on distance (depthThreshold), let's use avoltage threshold on the intensity: 
void LivingSpot::updateDepthThreshold(float range) { // this updates the depth_Threshold_Volts
	depthThreshold=range;
	depth_Threshold_Volts=computeIntensityVolts(depthThreshold, RedLaserPower); // this is unconstrained to the meaningful clipping ranges...
}

// ===================================================================================================
// -------------------------- CALIBRATION FUNCTION ------------------------------------------------
// This routine computes Attenuation_Factor and I_Volts_Background. 
// For this, A PAIR OF GOOD READOUTS is demanded.
// Calibration returns GOOD_CALIBRATION if laser power can be adjusted so as to get a redout for the intensity which is
// "exactly" equal to I_OPTIMAL_SAT= I_Volts_OPTIMAL_REF_FACTOR*I_Volts_SAT. Otherwhise, it will return FAIR_CALIBRATION if the intensity can be 
// adjusted between the proper limits PD_mW_UpperMarge and PD_mW_LowerMarge. If this is impossible, BAD_CALIBRATION is returned.
int LivingSpot::RangeCalibration(void) { // rem: exit is Proper_Calibration_Flag

	int i, num_loops;
	int storePowerCorrectionMode, storeRadiusCorrectionMode; //auxiliary variables
        float storeAper_DA;
	float meanIntensity;  // first and second moments of the measured intensity 

        storeAper_DA=Aper_DA; // otherwise, we could reset the radius aperture 
	storePowerCorrectionMode=PowerCorrectionMode;
	storeRadiusCorrectionMode=RadiusCorrectionMode;
	PowerCorrectionMode=POWER_FIXED; // to avoid being corrected by the CALIBRATION SACCADE.
	RadiusCorrectionMode=RADIUS_FIXED;// to avoid being corrected by the CALIBRATION SACCADE.

        // RESET Radius aperture? (not really necessary, but it may be a good idea):
	Aper_DA=180.0/PI*atan(R_cm_REF/depth_Range_REF)/IO.AD_to_Deg_MIRROR_X; // initialization of the current angular aperture of the saccade (card units).
	Aper_Degree=1.0*Aper_DA*IO.AD_to_Deg_MIRROR_X;

	//I_Volts_LowerMarge_REF_FACTOR=paramLOWER_REF_FACTOR;// usually around 0.7
	// I_Volts_OPTIMAL_REF_FACTOR=paramREF_FACTOR; // usually around 0.8
	//I_Volts_UpperMarge_REF_FACTOR=paramUPPER_REF_FACTOR;// usually around 0.9
	// rem: the idea is to mantain mean(I)+sigm(I)<Iref.
	// REM: in further versions, we can have a condition such that if I_Volts_LowerMarge_REF_FACTOR=-1 or/and 
	// I_Volts_UpperMarge_REF_FACTOR=-1, these parameters are CALCULATED BY MEASURING the actual variance of the signal.

	//The optimal REFERENCE intensity (to be "locked" by feedback if possible) is I_Volts_OPTIMAL. A closer value, I_Volts_REF will be the real lock. 
	//I_mW_SAT=I_Volts_SAT*Volts_To_mW; 
	I_Volts_OPTIMAL= I_Volts_OPTIMAL_REF_FACTOR*I_Volts_SAT; // rem: I_Volts_SAT is #defined in the hardwareIO class (could be a parameter here). 
	//I_mW_OPTIMAL=I_Volts_OPTIMAL*Volts_To_mW; 

	// 1) We will use a circular saccade to get data to process. 
	// Find a reasonable number of loops for the saccade (which contains _numSamplesCircularSaccade points):
	// for instance, let the whole process take 2 seconds: num_loops*_numSamplesCircularSaccade/sampling_Freq_DA=0.25...
	// Use an heuristic value: 16 points gives roughly
	// a 1ms saccade, so, to wait 1/4 seconds we need: 250ms = (_numSamplesCircularSaccade/16 ms)*num_loops
	//num_loops=250/_numSamplesCircularSaccade*16;
	num_loops=int(1.0*500/_numSamplesCircularSaccade*2);
	if (num_loops<1) num_loops=1;

	// START CALIBRATION:
	// First, set the red laser at high power for visibility (and green laser OFF)
	RedLaserPower=MAX_RedLaserPower; // the global variable (used in the scanning trajectory function). REM: MAX_RedLaserPower is #defined in the hardwareIO file
        IO.setRedPower_mW(MAX_RedLaserPower); // actually set it!
	IO.setGreenPower(LOW);
	IO.setMirrorsCenter();	// IMPORTANT: TO TAKE MEASURES, we need to set El and Az to 0.

	IO.clearLCD();
	IO.displayLineLCD("  CALIBRATION   ",1);
	IO.displayLineLCD("     START      ",2);
	IO.buzzerBip(100, 5);
	delay(100);

	IO.clearLCD();
	IO.displayLineLCD("Place target z= ",1);
	IO.displayLineLCD(depth_Range_REF,2);
	delay(500);

	IO.buzzerBip(100, 2);
	IO.clearLCD();
	IO.displayLineLCD("Measuring       ",1);
	IO.displayLineLCD("I_background... ",2);
	delay(100);

	// 0) First,MEASURE THE the background intensity (laser power REALLY OFF), which is the FIRST FITTING PARAMETER (beta):
	//IO.setRedPower_mW(MIN_RedLaserPower); // no need to set the red power here, it is done in the scanTrajectory() function
	IO.setGreenPower(LOW);
	RedLaserPower=0; // this is the CURRENT red laser power, used by function scanTrajectory(). Power will be set to 0 in that function, no need to call IO.setRedPower_mW(0);
	moveSpot(POS_AD_MIRROR_CALIB_X, POS_AD_MIRROR_CALIB_Y);// This will set Az and El. REM: IMPORTANT: TO TAKE MEASURES, we need to set El and Az to 0:
	for (i=0;i<num_loops;i++) scanTrajectory(_circularSaccade, _numSamplesCircularSaccade, Az, El, Aper_DA, 0); // this is to initialize the mirrors (just in case).
	meanIntensity=0;
	for (i=0;i<num_loops;i++) {
		// scan and acquired data (rem: the scanTrajectory routine will set the laser to the current RedLaserPower value, and it won't change it 
                // since PowerCorrectionMode=POWER_FIXED)
		scanTrajectory(_circularSaccade, _numSamplesCircularSaccade, Az, El, Aper_DA, 0);
		//computes "averageI_Volts", the average value of the intensity for the WHOLE saccade (rem: average_maxI_AD is the average for the "light" zone):
		analyzeScanData(_numSamplesCircularSaccade);
		meanIntensity+=1.0*averageI_Volts; //averageI_Volts is the average all over the saccade (not just white or black)
	}
	// compute the average redout (rem: for minimal laser power, this is just the intensity of the background signal!):
	//I_AD_background=1.0*meanIntensity/num_loops; // this is the current PD redout intensity, in card units.
	I_Volts_background=1.0*meanIntensity/num_loops;//(I_AD_background-zero_BIPOLAR)/oneVolt_AD_PD;
	// then, convert this quantity to mW (just to have an idea of the value in mW):
	//I_mW_background=1.0*Volts_To_mW*I_Volts_background; // REM: reading is bipolar... 
	//std::cout << "Measured optical background noise: " << I_mW_background*1000000 << " nW (" << I_Volts_background << " V)" <<endl;

	//record calibrated data:
	//calibratedParameter.backgroundIntensity = I_AD_background;
	//--> I_Volts_background_EEPROM 

	IO.buzzerBip(200, 1);
	IO.clearLCD();
	IO.displayLineLCD("I_Volts_backg:    ",1);
	IO.displayLineLCD(I_Volts_background,2);
	delay(500);

// 1) Second, measure maximum and minimum redout from the photodetector with maximum and minimum laser power (for the current distance depth_Range_REF)
	
// (a) MIN EMISSION (REM: we need to do this BEFORE the measure of MAX, because we will use I_Volts and sigmI_Volts in the while-loop of the optimal calibration
        IO.buzzerBip(200, 2);
	IO.clearLCD();
	IO.displayLineLCD("Measuring       ",1);
	IO.displayLineLCD("MIN value...    ",2);
	delay(100);

	RedLaserPower=MIN_RedLaserPower; //MIN_RedLaserPower is a #define in the hardwareIO class
	for (i=0;i<num_loops;i++) scanTrajectory(_circularSaccade, _numSamplesCircularSaccade, Az, El, Aper_DA, 0);
	meanIntensity=0;
	for (i=0;i<num_loops;i++) {
		// scan and acquired data:
		scanTrajectory(_circularSaccade, _numSamplesCircularSaccade, Az, El, Aper_DA, 0);
		//computes "averageI_Volts", the average value of the intensity for the WHOLE saccade (rem: average_maxI_AD is the average for the "light" zone):
		analyzeScanData(_numSamplesCircularSaccade);
		meanIntensity+=1.0*averageI_Volts;	
	}
	// compute the average redout (rem: for minimal laser power, this is just the intensity of the background signal!):
	//I_AD_minLaser=1.0*meanIntensity/num_loops; // this is the current PD redout intensity, in card units.
	I_Volts_minLaser=1.0*meanIntensity/num_loops;//1.0*(I_AD_minLaser-zero_BIPOLAR)/oneVolt_AD_PD;
	//I_mW_minLaser=1.0*Volts_To_mW*I_Volts_minLaser; // REM: reading is bipolar... 

	IO.buzzerBip(200, 1);
	IO.clearLCD();
	IO.displayLineLCD("I_Volts_min:    ",1);
	IO.displayLineLCD(I_Volts_minLaser,2);
	delay(1000);

	
  	// (b) MAX EMISSION:
        IO.buzzerBip(200, 2);
	IO.clearLCD();
	IO.displayLineLCD("Measuring       ",1);
	IO.displayLineLCD("MAX value...    ",2);
	delay(100);

	//IO.setRedPower_mW(MAX_RedLaserPower); // this is the CURRENT laser powers, used by function Saccade();
	RedLaserPower=MAX_RedLaserPower; //RedLaserPower is the CURRENT laser power (will be set in the scanTrajectory function)
	for (i=0;i<10;i++) scanTrajectory(_circularSaccade, _numSamplesCircularSaccade, Az, El, Aper_DA, 0); // this is to initialize the mirrors (just in case).
	meanIntensity=0;
	for (i=0;i<num_loops;i++) {
		// scan and acquired data:
		scanTrajectory(_circularSaccade, _numSamplesCircularSaccade, Az, El, Aper_DA, 0);
		//computes "averageI_Volts", the average value of the intensity for the WHOLE saccade (rem: average_maxI_AD is the average for the "light" zone):
		analyzeScanData(_numSamplesCircularSaccade);
		meanIntensity+=1.0*averageI_Volts; 
	}
	// compute the average redout:
	//I_AD_maxLaser=1.0*meanIntensity/num_loops; // this is the current PD redout intensity, in card units.
	// Convert the redout into Volts and mW 
	I_Volts_maxLaser=1.0*meanIntensity/num_loops;//1.0*(I_AD_maxLaser-zero_BIPOLAR)/oneVolt_AD_PD;
	// then, convert this quantity to mW (Volts_To_mW is the ONLY variable that depends on the lockin calibration parameters!)
	//I_mW_maxLaser=1.0*Volts_To_mW*I_Volts_maxLaser; // REM: reading is bipolar... 

	//record calibrated data.
	//calibratedParameter.maxIntensity = I_AD_maxLaser;

	IO.buzzerBip(200, 1);
	IO.clearLCD();
	IO.displayLineLCD("I_Volts_max:    ",1);
	IO.displayLineLCD(I_Volts_maxLaser,2);
	delay(500);

        IO.buzzerBip(200, 1);
	IO.clearLCD();
        //IO.displayLineLCD("I_Volts_max:    ",1);
	IO.displayLineLCD  ("sigmI_Volts max:",1);
	IO.displayLineLCD(sigmI_Volts,2);
	delay(500);

	// End of power measures. Switch off lasers:
	IO.setRedPower_mW(MIN_RedLaserPower); // no need to set the red power here, it is done in the scanTrajectory() function
	IO.setGreenPower(LOW);

        // Acceptable range for computing the value of the Proper_Intensity_Flag:
	I_Volts_UpperMarge=min(I_Volts_UpperMarge_REF_FACTOR*I_Volts_SAT, I_Volts_SAT-sigmI_Volts); 
	I_Volts_LowerMarge=max(I_Volts_LowerMarge_REF_FACTOR*I_Volts_SAT, I_Volts_background);//actually the second parameter should be: I_Volts_background+sigmI_Volts_background

	// HERE, we can already determine if the Calibration is GOOD, FAIR or BAD:
	// (rem: Attenuation_Factor is computed at the end of all subcases)
	if ((I_Volts_maxLaser<I_Volts_LowerMarge)||(I_Volts_minLaser>I_Volts_UpperMarge)) {
		Proper_Calibration_Flag=BAD_CALIBRATION;	
		//std::cout << "BAD CALIBRATION: ";  
		if (I_Volts_maxLaser<I_Volts_LowerMarge) {
			//std::cout << "Calibration target TOO FAR to take a good measure." << endl;
			IO.buzzerBip(200, 5);
			IO.clearLCD();
			IO.displayLineLCD(" TARGET TOO FAR ",1);
			IO.displayLineLCD("  (START AGAIN) ",2);
			delay(500);

			// Attenuation_Factor and lockin intensity reference (I_Volts_REF) are computed anyway, assuming the best measure possible (max power of laser):
			RedLaserPower_REF=MAX_RedLaserPower;
			I_Volts_REF=I_Volts_maxLaser;
			// Before anything else, let's compute I_Volts_REF in other units, for presentation purposes:
			//I_mW_REF=I_Volts_REF*Volts_To_mW;
			//I_AD_REF=I_Volts_REF*oneVolt_AD_PD+zero_BIPOLAR;	
		}
		else
		{
			//std::cout << "Calibration target TOO CLOSE to take a good measure." << endl;
			IO.buzzerBip(200, 5);
			IO.clearLCD();
			IO.displayLineLCD("TARGET TOO CLOSE",1);
			IO.displayLineLCD("  (START AGAIN) ",2);
			delay(500);

			// Attenuation_Factor is computed anyway, assuming the best measure possible (max power of laser):
			RedLaserPower_REF=MIN_RedLaserPower;
			I_Volts_REF=I_Volts_minLaser;
			// Before anything else, let's compute I_Volts_REF in other units, for presentation purposes:
			//I_mW_REF=I_Volts_REF*Volts_To_mW;
			//I_AD_REF=I_Volts_REF*oneVolt_AD_PD+zero_BIPOLAR;
		}	
	}
	else { 
		if ((I_Volts_maxLaser>I_Volts_LowerMarge)&&(I_Volts_maxLaser<I_Volts_OPTIMAL)) {
			Proper_Calibration_Flag=FAIR_CALIBRATION;
			//std::cout << "FAIR CALIBRATION (but calibration target a LITTLE TOO FAR)" << endl;
			IO.buzzerBip(200, 5);
			IO.clearLCD();
			IO.displayLineLCD("FAIR CALIBRATION",1);
			IO.displayLineLCD("little too far",2);
			delay(500);	

			RedLaserPower_REF=MAX_RedLaserPower; // MAX LASER POWER (this will produce a readout I_mW equal to I_mW_maxLaser for depth_Range_REF, as measured above)
			I_Volts_REF=I_Volts_maxLaser;// this is the best that can be done, but still I_Volts_maxLaser is smaller than I_Volts_OPTIMAL
			// Before anything else, let's compute I_Volts_REF in other units, for presentation purposes:
			//I_mW_REF=I_Volts_REF*Volts_To_mW;
			//I_AD_REF=I_Volts_REF*oneVolt_AD_PD+zero_BIPOLAR;
		}

		if ((I_Volts_minLaser>I_Volts_OPTIMAL)&&(I_Volts_minLaser<I_Volts_UpperMarge)) {
			Proper_Calibration_Flag=FAIR_CALIBRATION;
			//std::cout << "FAIR CALIBRATION (but calibration target a LITTLE TOO CLOSE)" << endl;
			IO.buzzerBip(200, 5);
			IO.clearLCD();
			IO.displayLineLCD("FAIR CALIBRATION",1);
			IO.displayLineLCD("little too close",2);
			delay(500);

			RedLaserPower_REF=MIN_RedLaserPower; // MIN LASER POWER, this will produce a readout I_mW equal to I_mW_minLaser (as measured above)
			I_Volts_REF=I_Volts_minLaser;// this is the best that can be done, but still I_Volts_minLaser is larger than I_Volts_OPTIMAL
			// Before anything else, let's compute I_Volts_REF in other units, for presentation purposes:
			//I_mW_REF=I_Volts_REF*Volts_To_mW;
			//I_AD_REF=I_Volts_REF*oneVolt_AD_PD+zero_BIPOLAR;
		}

		// GOOD CALIBRATION CASE:
		// Let's compute the Attenuation factor (for this, we need to find the RedLaserPower_REF corresponding to a redout of I_Volts_OPTIMAL:
		if ((I_Volts_maxLaser>=I_Volts_OPTIMAL)&&(I_Volts_minLaser<=I_Volts_OPTIMAL)) {
			Proper_Calibration_Flag=GOOD_CALIBRATION;
			//std::cout << "OPTIMAL CALIBRATION POSSIBE. Now computing..." << endl;
			IO.buzzerBip(200, 5);
			IO.clearLCD();
			IO.displayLineLCD("START OPTIMAL   ",1);
			IO.displayLineLCD("CALIBRATION.....",2);
			delay(500);

			// Set the reference intensity to the (attainable!!) optimum:
			I_Volts_REF=I_Volts_OPTIMAL;
			// Before anything else, let's compute I_Volts_REF in other units, for presentation purposes:
			//I_mW_REF=I_Volts_REF*Volts_To_mW;
			//I_AD_REF=I_Volts_REF*oneVolt_AD_PD+zero_BIPOLAR;

			// Now, this case is the only one when we KNOW that the optimal value for the reference is attainable (I_Volts_REF=I_Volts_OPTIMAL)
			// but we don't know what is the corresponding value of the laser power, RedLaserPower_REF. It has to be searched,
			// using an iterative method - here, the BISECTION method (the model cannot be used, because Attenuation_Factor has not been calculated yet)

			// a) first, set initial value for the laser:
			RedLaserPower=MIN_RedLaserPower; // which we know produces a readout I_Volts_minLaser SMALLER or EQUAL to I_Volts_OPTIMAL
			int sign_old=-1; //  = 2*((I_Volts_minLaser-I_Volts_OPTIMAL)>0)-1, this is the "sign" of (I_Volts_minLaser-I_Volts_OPTIMAL). 
			//Attn with "0", it is considered "-"
			// (we don't initialize RedLaserPower_REF to MAX_RedLaserPower, because the readout could be saturated, making it difficult to 
			// compute an appropiate value for the delta_LaserPower.
			delta_RedLaserPower=0.2*(MAX_RedLaserPower-MIN_RedLaserPower);// initialization of the power correction.
			//Rem: the way the readout varies is LINEAR with the laser power. The constant is known (I_Volts_background), and is supposed to be very small; 
			// however, the linear factor is not known (Attenuation_Factor). 
			// rem: sigmI_Volts is calculated at each Saccade(), and even if initial value is very small (i.e, signal saturated), it should work 
                        //as a convergence test because it keeps changing.
			while ((abs(delta_RedLaserPower)>=min_delta_RedLaserPower)&&(abs(I_Volts-I_Volts_OPTIMAL)>sigmI_Volts)) {
				RedLaserPower+=delta_RedLaserPower;
				// compute average readout (as well as variance of the readout, assuming the saccade FULL INSIDE the calibration target)
				for (i=0;i<1;i++) scanTrajectory(_circularSaccade, _numSamplesCircularSaccade, Az, El, Aper_DA, 0); // this is to initialize the mirrors (just in case).
                                meanIntensity=0;
				for (i=0;i<num_loops;i++) {
					// scan and acquired data:
					scanTrajectory(_circularSaccade, _numSamplesCircularSaccade, Az, El, Aper_DA, 0);
					//computes "averageI_Volts", the average value of the intensity for the WHOLE saccade (rem: average_maxI_AD is the average for the "light" zone):
					analyzeScanData(_numSamplesCircularSaccade);
					meanIntensity+=1.0*averageI_Volts; //averageI_Volts is the average value of the intensity for the whole saccade
				}
				// compute the average redout (rem: for minimal laser power, this is just the intensity of the background signal!):
				//I_AD=1.0*meanIntensity/num_loops; // this is the current PD redout intensity, in card units.
				I_Volts=meanIntensity/num_loops; //1.0*(I_AD-zero_BIPOLAR)/oneVolt_AD_PD;
				//I_mW=1.0*Volts_To_mW*I_Volts; // REM: reading is bipolar... 

				if (((2*((I_Volts-I_Volts_OPTIMAL)>0)-1)*sign_old)==-1) { 
					delta_RedLaserPower*=-.6; // when sign changes (otherwise delta_RedLaserPower does NOT change neither in value nor in sign)
					sign_old*=-1;
				}
			} // end of root-finding BISECTION procedure 

                        RedLaserPower_REF=RedLaserPower;
			I_Volts_REF=I_Volts; // if we have enough precision (for the laser and detector), this should be sensibly equal to I_Volts_OPTIMA. 
			// Before anything else, let's compute I_Volts_REF in other units, for presentation purposes:
			//I_mW_REF=I_Volts_REF*Volts_To_mW;
			//I_AD_REF=I_Volts_REF*oneVolt_AD_PD+zero_BIPOLAR;

			//record calibrated data.
			//calibratedParameter.intensity = I_AD;
			//calibratedParameter.deltaRedLaserPower= delta_RedLaserPower;
			//std::cout << "I_AD:" << I_AD << endl;
			
			//IO.buzzerBip(200, 5);
			//IO.clearLCD();
			//IO.displayLineLCD("OPTIMAL Iv=     ",1);
			//IO.displayLineLCD(I_Volts_REF,2);
			//delay(1000);

			//std::cout << "The reference intensity is locked to: " <<  I_Volts_REF << " V ("<< I_mW_REF*1000000 << " nW)"  << endl;
			//std::cout << "(optimal was: " <<  I_Volts_OPTIMAL << " V (" <<  I_mW_OPTIMAL*1000000  << " nW)" << endl;
		}

	}

	// Now we can proceed with the computation of the fitting parameter Attenuation_Factor (alpha):
	switch(Distance_Intensity_Model) {
		case PRE_CALIBRATED_SKIN: 
			// use pre-calibrated model (26.2.2005), on skin (hand):
			Attenuation_Factor=Attenuation_Factor_DEFAULT;
			I_Volts_background=I_Volts_background_DEFAULT; 
			//std::cout << "Pre-Calibrated Target Attenuation (skin): " <<  Attenuation_Factor << " (1/cm2)"  << endl;
			IO.buzzerBip(200, 5);
			IO.clearLCD();
			IO.displayLineLCD("(PRE-CALIB MODE)",1);
			IO.displayLineLCD(Attenuation_Factor,2);
			delay(500);

			break;
			// in all the other cases, use the model to find the parameters, and do not touch to the measured I_Volts_background
			// REM: HERE, we assume El=0 and Az=0.
		case  ORDER_0: 	
		case  ORDER_0_CORRECTED_LENS:
			// (3) OLD MODEL (too simple):
			Attenuation_Factor=(I_Volts_REF-I_Volts_background)*pow(depth_Range_REF+range_Adjust,2)/RedLaserPower_REF;
			//REM: range_Adjust is a parameter "ad-hoc" that was found using the fitting DitanceCm_To_Volts.m (around 8cm)
			//std::cout << "Measured Target Attenuation: " <<  Attenuation_Factor << " (cm-2)"  << endl;
			IO.buzzerBip(200, 5);
			IO.clearLCD();
			//IO.displayLineLCD("(PRE-CALIB MODE)",1);
			IO.displayLineLCD("(O/CORRECT LENS)",1);
			IO.displayLineLCD(Attenuation_Factor,2);
			delay(500);
			break;
		case ORDER_1:// (2) The really meaninful, physically based model:
		case ORDER_1_NO_COSINUS: // this is a model which does not include a cos relation for the photodetector. The model is otherwise "exact" and
			// depth_Range can be exactly resolved as in ORDER_1_CORRECTED_LENS, by solving a second degree equation.
			//(Attenuation factor is calculated as in ORDER_1, physically based model)
			Attenuation_Factor=(I_Volts_REF-I_Volts_background)/RedLaserPower_REF/(pow(depth_Range_REF,-2)-3/2*(PD_X*PD_X+PD_Y*PD_Y)*pow(depth_Range_REF,-4));
			//std::cout << "Measured Target Attenuation: " <<  Attenuation_Factor << " (cm-2)"  << endl;
			IO.buzzerBip(200, 5);
			IO.clearLCD();
			//IO.displayLineLCD("(PRE-CALIB MODE)",1);
			IO.displayLineLCD("(1/NO SINUS)",1);
			IO.displayLineLCD(Attenuation_Factor,2);
			delay(500);
			break;
		case ORDER_1_CORRECTED_LENS:
			// (2) Ad-Hoc corrected physically based mode, to include lens in front of PD (relation as cos^2 instead of cos):
			Attenuation_Factor=1.0*(I_Volts_REF-I_Volts_background)/RedLaserPower_REF*pow((PD_X*PD_X+PD_Y*PD_Y)/depth_Range_REF+depth_Range_REF,2); 
			//std::cout << "Measured Target Attenuation: " <<  Attenuation_Factor << " (cm2)"  << endl;
			IO.buzzerBip(200, 5);
			IO.clearLCD();
			//IO.displayLineLCD("(PRE-CALIB MODE)",1);
			IO.displayLineLCD  ("Att- 1 CORRLENS)",1);
			IO.displayLineLCD(Attenuation_Factor,2);
			delay(500);
			break;
		default:
			break;
	}

	// Now, we can compute other things: 
	//(1) The minimum contrast to be able to discriminate zones, even in autothreshold mode
	MIN_CONTRAST_Volts=MIN_CONTRAST_FACTOR*(I_Volts_REF-I_Volts_background);
	//MIN_CONTRAST_AD=MIN_CONTRAST_Volts*oneVolt_AD_PD;
	//MIN_CONTRAST_mW=MIN_CONTRAST_Volts*Volts_To_mW;

	// CALIBRATED MAXIMUM and MINIMUM WORKING DISTANCES. Rem: Minimum and maximum ranges including oclusion problems due to the geometry 
	// of the setup are defined in MANUAL_MIN(and MAX)_DEPTH_RANGE.
	// CALIBRATED_MAX_DEPTH_RANGE is the distance that can be reached by putting the laser at maximum power and
	// gives a signal I_Volts still LARGER than I_Volts_LowerMarge:
	CALIBRATED_MAX_DEPTH_RANGE=ComputeRangeUnconstrained(I_Volts_LowerMarge, MAX_RedLaserPower);
	// CALIBRATED_MIN_DEPTH_RANGE is the maximal proximity without saturation (in principle, this could be equal to epsilon, but
	// the laser cannot be adjusted to almost zero with good precision. In fact, minimum laser power has been set to MIN_RedLaserPower). 
	CALIBRATED_MIN_DEPTH_RANGE=ComputeRangeUnconstrained(I_Volts_UpperMarge, MIN_RedLaserPower);

	// We can compute now the absolute maximum and minimum acceptable ranges:
	MAX_DEPTH_RANGE=min(MANUAL_MAX_DEPTH_RANGE, CALIBRATED_MAX_DEPTH_RANGE);
	MIN_DEPTH_RANGE=max(MANUAL_MIN_DEPTH_RANGE, CALIBRATED_MIN_DEPTH_RANGE);

	// SETTING THE FIXED THRESHOLD (this is optional: we may not want to change it here)
        // (used when thresholding_Mode  state variable = FIXED_THRESHOLD). 
        // NOTE: this value can be changed by hand afterwards (fixed_Threshold_Volts is a public variable). 
	fixed_Threshold_Volts=(I_Volts_REF-I_Volts_background)*FIXED_THRESHOLD_FACTOR+I_Volts_background;
	//fixed_Threshold_AD=fixed_Threshold_Volts*oneVolt_AD_PD+zero_BIPOLAR;
	//fixed_Threshold_mW=fixed_Threshold_Volts*Volts_To_mW;

	// Restore correction modes and other things:
	PowerCorrectionMode=storePowerCorrectionMode;
	RadiusCorrectionMode=storeRadiusCorrectionMode;
        Aper_DA=storeAper_DA; // this is particularly interesting in the case that the radius was not set to auto-correction mode. 

	// restore initial position of mirrors to DEFAULT:
	moveSpot(Az_Default, El_Default);

	//Laser powers properly set in case a tracking starts from the current (calibrating) position:
	RedLaserPower=RedLaserPower_REF; 

        IO.buzzerBip(200, 5);
	IO.clearLCD();
        //IO.displayLineLCD("OPTIMAL Iv=     ",1);
	IO.displayLineLCD("Ref. Iv=        ",1);
        IO.displayLineLCD(I_Volts_REF,2);
	delay(500);

	return(Proper_Calibration_Flag);
}




// ===================================================================================================

// -------------- LASER POWER UPDATE -------------------------------------------------------------------------
// Computation of delta_RedLaserPower, in order to mantain the readout to I_Volts_REF:
float LivingSpot::LaserPowerCorrection(float paramIVoltsMeasured, float paramRedLaserPower) {	
	float powercorrection; 	 

        // Note: we wouldn't need to check Proper_Intensity_Flag here: we always should have I_Volts>I_Volts_background, but it can happens if environment changes
       // (in which case is better to re-calibrate). Anyway, let's check it (so as to avoid checking the case when I_Volts-I_Volts_background<0): 
         //... TO THINK ABOUT: if I_Volts==I_Volts_background (or if it is very close) we cannot measure (or we cannot measure properly) the linear coef. at that distance, 
         // between laser power and measured intensity. Our best gess would be to put all the power, but not too much as we can saturate, and then we need to slowly converge to the optimal 
        // value. This is not good in case that there was a "hole" or some dark spot - it will take more time to go back to normal. Another method is to just ADD a constant power (until 
        // saturation perhaps). This way, we will explore all the possible powers. 
         if (paramIVoltsMeasured<=I_Volts_background) {
            // First method: 
             powercorrection = 10*min_delta_RedLaserPower; // rem: I_Volts_background is very small, so we always assume the linear relation cannot be properly computed. 
            // NOte: paramIVoltsMeasured<=I_Volts_background is equivalent to Proper_Intensity_Flag=TOO_WEAK, but it is better to do this here in case we use the function alone. 
            // Second method: (faster, but can be generate delays in going back to normal if the darkness was very momentary): 
             // powercorrection = MAX_RedLaserPower- delta_RedLaserPower;
         }
        else {
	  // (1) Assuming a linear relation between laser power and detected intensity (this menas that a linear relation is assumed between
	  // the voltage applied to the laser and its output power, plus a linear relation between the detected power and the readout of the lock-in):
	  powercorrection=LaserCorrectionDamping*((I_Volts_REF-I_Volts_background)/(paramIVoltsMeasured-I_Volts_background)-1)*paramRedLaserPower; 
         }
         // NOTE: A MORE VERSATILE WAY TO MANTAIN THIS I_Volts_RED not relying on a linear relation between laser power and detected intensity should 
	// use a "gradient descent" method: if the laser-to-intensity curve is not perfectly linear, we can still assume that the laser power was close to the 
	// right value, so we can linearize the laser-intensity curve.
         
	return(powercorrection); 
}

// Update laser power (using the current depth range, and taking into account the intensity redout limits (noise)).
// We can call just this function, and delta_RedLaserPower as well as the current laser power RedLaserPower will be updated. 
// Note: it must be called after depth is computed (not using the 'unconstrained' method):
int LivingSpot::correctLaserPower(void) {
	int returnValue=1;
	
        // (a) first, update the delta_RedLaserPower: (regardeless of the signal strength on Proper_Intensity_Flat):
	 delta_RedLaserPower=LaserPowerCorrection(I_Volts, RedLaserPower); // I_Volts is the current "global" intensity readout for the saccade, 
        //and RedLaserPower the current laser power for that readout. 
	 
        // (b) then, update the current laser power: 
	RedLaserPower+=delta_RedLaserPower; 
	// this is to keep on the REAL limits, so depth can be computed correctly using the readout from photodetector:
	if (RedLaserPower>MAX_RedLaserPower) {RedLaserPower=MAX_RedLaserPower; returnValue=-1;} 
	if (RedLaserPower<MIN_RedLaserPower) {RedLaserPower=MIN_RedLaserPower; returnValue=-1;}
	
	return(returnValue); // returns 1 if we actually modified something, 0 if there was no need to modify anything, and -1 if we modified, but laser output was clipped
}

// ===================================================================================================

// -------------- SACCADE RADIUS UPDATE -------------------------------------------------------------------------
// Setting the radius of the saccade (not the aperture!) in cm. Of course, since this value is meant to be constant, it means that at the reference 
// distance, it will be the same; that means that this value is the value of R_cm_REF.
// If the program is set to radius correction ON, then this will be the "real" radius of the saccade
int LivingSpot::setRadiusSaccade(float pRcm) {  
	int returnvalue=-1;
        // First, we need to convert to Aper_DA to check the limits (we don't care if this is not the correct aperture for the current distance - this will be automatically 
        // changed if the radius correction is set to ON). But we need to compute it here to test the limits of the input radius, ASSUMING THIS IS THE SIZE
        // OF THE RADIUS AT THE REFERENCE DISTANCE:
        float Aper_Rad=atan(pRcm/depth_Range_REF); // auxiliary variable (not global now)
        float auxAper_Degree=180.0/PI*Aper_Rad; // re-initialization of the current angular aperture of the saccade from reference distance and saccade size
	float auxAper_DA=1.0*auxAper_Degree/IO.AD_to_Deg_MIRROR_X; // re-initialization of the current angular aperture of the saccade (card units).
	
        if ((auxAper_DA>0)&&(auxAper_DA<MAXRADIUS)) {
                R_cm_REF=pRcm;
		Aper_DA=auxAper_DA;
		Aper_Degree=Aper_DA*IO.AD_to_Deg_MIRROR_X; // note: we only use the conversion from AD to degrees for the X mirror
		//Aper_Rad=Aper_Degree*PI/180; not global anymore
		returnvalue=0;
		// rem: no need to recalculate the saccade table! this must be done only when changing the number of samples
	} // else don't change anything and return -1
	return(returnvalue); // returns -1 if requested radius was out of range, and 0 if we could perform the modification
}

// Manual change of the "REFERENCE" saccade aperture (directly in AD units) (this is similar to setRadiusSaccade, but using AD units).
// THIS IS THE aperture of the saccade when it is at the reference distance depth_Range_REF. 
int LivingSpot::setAperSaccade(float pAperDA) {  // will change the current (global) variable aperture (in fact, the SIZE of the drawing): Aper_DA (which is a float)
	int returnvalue=-1;
	if ((pAperDA>0)&&(pAperDA<MAXRADIUS)) {
		Aper_DA=pAperDA;
		Aper_Degree=Aper_DA*IO.AD_to_Deg_MIRROR_X; // note: we only use the conversion from AD to degrees for the X mirror
		float Aper_Rad=1.0*Aper_Degree*PI/180;// not global anymore
                
                //now, change teh reference radius:
                R_cm_REF=depth_Range_REF*tan(Aper_Rad);
                
                // REM: normally, we should recompute Aper_AD based on the current distance (but this will be done if set to radius correction; otherwise,
                // this will be the FIXED aperture!. 
		returnvalue=0;
		// rem: no need to recalculate the saccade table! this must be done only when changing the number of samples
	} // else don't change anything and return -1
	return(returnvalue); // returns -1 if requested radius was out of range, and 0 if we could perform the modification
}

void LivingSpot::resetRadius(void) { // will change global variable Aper_DA and Aper_Degree
	float Aper_Rad=atan(R_cm_REF/depth_Range_REF); // auxiliary variable (not global now)
	Aper_Degree=180.0/PI*Aper_Rad; // re-initialization of the current angular aperture of the saccade from reference distance and saccade size
	Aper_DA=1.0*Aper_Degree/IO.AD_to_Deg_MIRROR_X; // re-initialization of the current angular aperture of the saccade (card units).
      /// Example: for R_cm_REF=1 cm and depth_Range_Ref=50 cm, we have Aper_DA=114 (assuming an excursion of -20 to 20 degrees for 0 to 4000 for the mirror control)
} 

// Computes the correction for the radius, given a certain depth range (rem: this is done while checking for appropriate measured intensity, Proper_Intensity_Flag): 
float LivingSpot::computeRadiusCorrection(float paramdepth_Range) { // input is the current depth_Range, output is delta_Aper_AD
	float correction;
	// this computes the delta_Aper_AD, i.e. the quantity required to adjust the radius of the saccade as a function of the computed distance
	// rem: the routine does not modify the actual value of the radius, this is done in the function Track.
	// The current angular aperture of the saccade (in card units) is Aper_DA, the reference radius aperture (in cm) is R_cm_REF
	// The new current radius could be updated immediatly (Aper_DA=oneDegree_DA*180.0/pi*atan(R_cm_REF/depth_Range), but we can give the choice of a damping factor: 
	
        //if (Proper_Distance_Flag==PROPER_RANGE) ...// no need to check this if range was properly clipped!
        //else if (Proper_Distance_Flag==TOO_FAR) ...  
	
        correction=RadiusCorrectionDamping*(180.0/PI*atan(R_cm_REF/paramdepth_Range)/IO.AD_to_Deg_MIRROR_X-Aper_DA);
   
	return(correction); //Aper_DA is the current radius aperture (rem: oneDegree_DA is taken for the X axis only)
}

// update radius from the current computed radius correction, with limit control
void LivingSpot::updateRadius() { 
	if ((Aper_DA+delta_Aper_AD>0)&&((Az+Aper_DA+delta_Aper_AD)<xMax)&&((Az-Aper_DA-delta_Aper_AD)>xMin)&&((El+Aper_DA+delta_Aper_AD)<yMax)&&((El-Aper_DA-delta_Aper_AD)>yMin)) {
		Aper_DA+=delta_Aper_AD;
		Aper_Degree=1.0*Aper_DA*IO.AD_to_Deg_MIRROR_X;
	} // otherwise the Aper_DA values are unchanged.
        if (Aper_DA>MAXRADIUS) Aper_DA=MAXRADIUS;
}

// ===========================================================================================================================
// ---------- SACCADE TABLE GENERATION (we could put her the letter saccade position table generation too...) ----------------

/*
//Creates and updates the tables related to the CIRCULAR saccade when we change the number of samples (not the radius)
int LivingSpot::createCircularSaccade(int numSamples) {
	if (numSamples > MAX_SACCADE_SAMPLES) return(-1); // this is important: we may have memory leaks otherwise!

	//Update the total number of samples (should be less than the max number of samples, MAX_SACCADE_SAMPLES (throw error?)
	_numSamplesCircularSaccade = numSamples;
	//compute the position of each sampling spot of the circular saccade
	float angle_pie = 2.0 * PI / _numSamplesCircularSaccade;
	for(int i=0; i<_numSamplesCircularSaccade; i++){
		_circularSaccade[2 * i] = cos(angle_pie * i);
		_circularSaccade[2 * i + 1] = sin(angle_pie * i);
	}
	return(0);
}
*/

//Creates and updates the tables related to the CIRCULAR saccade when we change the number of samples.
int LivingSpot::createCircularSaccade(int numSamples) {
  	//Update the total number of samples (should be less than the max number of samples, MAX_SACCADE_SAMPLES:
	if (numSamples > MAX_SACCADE_SAMPLES) return(-1); // this is important: we may have memory leaks otherwise!
	_numSamplesCircularSaccade = numSamples;
	//compute the position of each sampling spot of the circular saccade (store as bytes! from 0 to 255, central position being 127)
	float angle_pie = 2.0 * PI / _numSamplesCircularSaccade;
	for(int i=0; i<_numSamplesCircularSaccade; i++){
		_circularSaccade[2 * i] = byte(127.0*cos(angle_pie * i)+127);
		_circularSaccade[2 * i + 1] = byte(127.0*sin(angle_pie * i)+127);
	}
	return(0);
}

// ===========================================================================================================================

//Performs a scanning saccade centered on the current x and y, and gets the data (mesure the lockIn amplifier value for each sampling spot)
// Note: this function is just like a draw function on OpenGL - we could have a PERSPECTIVE and MODELVIEW matrix applied to it!!!!
// IN THE FUTURE, the scanTrajectory will be a METHOD of the trajectory object; or we can have a global function that applies to trajectory structures (if we don't want to make objects).
int LivingSpot::scanTrajectory(byte *saccadePosCenteredAndNormalized, int numsamples, float centAz, float centEl, float radius, float rotation) {  

  //NOTE: the order of the operations below may seem arbitrary, but it has been chosen to optimized speed and good match between laser on/off delays and mirror positioning
  // Stop the lasers before centering of the mirrors:
    IO.setRedPower_mW(MIN_RedLaserPower);
    IO.setGreenPower(LOW);
  
   // Set mirrors in start position (to avoid a line between the spots): 
        // We do FIRST resizing, then rotation, and then translation:
        float  x = radius*(saccadePosCenteredAndNormalized[0]-127)/127.0;
        float  y = radius*(saccadePosCenteredAndNormalized[1]-127)/127.0;
	IO.setMirrorsXY_AD(int(cos(rotation)*x-sin(rotation)*y+centAz),int(sin(rotation)*x+cos(rotation)*y+centEl));
         delayMicroseconds(DELAY_MIRRORS_START);
  
  // Update laser power using current delta_RedLaserPower (note: the place of this piece of code here is controversial... I am putting it 
	// here, because most behaviours that use the saccade function will also need to set the laser power (of course, depending on the laser update power mode): 
	// (REM: delta_RedLaserPower is computed regardless of the powercorrection mode in the preprocess saccade function)
        if (PowerCorrectionMode==POWER_ADJUST) correctLaserPower(); 
          // note: correctLaserPower returns 1 if we actually modified something, 0 if there was no need 
	// to modify anything, and -1 if we modified, but laser output was clipped (we could indicate this on the LCD display or something). It uses the CURRENT values
	// of depth_Range.
  
        // SET THE LASER POWERS are required by the laser power correction (red laser), or by the user mode (for the green)
	IO.setRedPower_mW(RedLaserPower); // this function (also) performs clipping
	IO.setGreenPower(GreenLaserPower);

	// Correct radius size (actually SIZE of ANY drawing!!) if required by the radius correction mode:
	if (RadiusCorrectionMode==RADIUS_ADJUST) updateRadius(); // update radius use the default R_cm_REF (which can be changed at any time by the user, and which is supposed to be the 
	// size of the radius when the target is at the reference distance )
	// Note: if there is no update of the radius, then it will keep the current value (that can be changed by hand)

          
  #ifdef NO_HARDWARE
   Serial.print('S');
 #endif	

	// *** NOTE *** we could have a PERSPECTIVE and MODELVIEW matrix applied to the saccadePosCenteredAndNormalized array, and create an auxiliary array that is displayed afterwards. 
	// We don't do this for the time being, but is a fantastic idea (we could copy OpenGL rendering pipeline. It will work because even though Arduino is not a dedicated matrix processor, 
	// we have to remember that we are not dealing with million of texels, but with just a few points per saccade. 
	// Interestingly, for perspective unwarping we could compute the modelview matrix for local flat surfaces during the depth scan itself, and then it would be easy to dewarp the image (this 
	// is done in a very simple way in the case of the varying size of the saccade with distance!) 
        
        // First, give time to the mirrors to position on the first point (in case there was some other spots, or the mirror had stop at the last saccade processing because of 
        // time consuming inter-saccade calculations. This can be done by two methods:  
        // (a) Place the mirror and wait: 
          //IO.setMirrorsXY_AD(int(1.0*centAz + saccadePosCenteredAndNormalized[0]*radius), int(1.0*centEl + saccadePosCenteredAndNormalized[1]*radius));
          //delayMicroseconds(DELAY_MIRRORS_START);
        //delayMicroseconds(1000);
        // (b) do extra samples (#define EXTRA_MIRROR_SAMPLES), that will be used to substitute the first points (that can be bad). This can be easy to do using the modulo function: 
        
        // REM: as for 20.7.2010, I will avoid an auxiliary array (But if having memory, I prefer my more elegant method, to be copied from versions of the program < 14). 
        register byte j;
        if (rotation!=0) {
           for (byte i=0; i<numsamples+EXTRA_MIRROR_SAMPLES; i++) {
             j=i%numsamples;
             // We do FIRST resizing, then rotation, and then translation:
             x = radius*(saccadePosCenteredAndNormalized[2 * j]-127)/127.0;
             y = radius*(saccadePosCenteredAndNormalized[2 * j + 1]-127)/127.0;
             // Position the mirrors: 
	     IO.setMirrorsXY_AD(int(cos(rotation)*x-sin(rotation)*y+centAz),int(sin(rotation)*x+cos(rotation)*y+centEl));
         
             // Inter point delay (the larger it is, the more the measurement will actully correspond to the sample position, and not to the arc between consecutive positions)
	    delayMicroseconds(DELAY_MIRRORS_CONTINUOUS); //must calibrate the timing
            

	    //if (i>=EXTRA_MIRROR_SAMPLES) _rawScanData[j] = IO.LockInRead_Volts(); // if to avoid calling the LockInRead on the points to be discareded
            _rawScanData[j] = IO.LockInRead_Volts(); // if to avoid calling the LockInRead on the points to be discareded
            
           }
         } else { // no rotation: this is useful for accelerating computation in the case of a circular saccade that does need to be rotated:
            for (byte i=0; i<numsamples+EXTRA_MIRROR_SAMPLES; i++) {
              j=i%numsamples;
             // We do FIRST resizing, then translation:
             IO.setMirrorsXY_AD(int(radius*(saccadePosCenteredAndNormalized[2 * j]-127)/127.0+centAz),int(radius*(saccadePosCenteredAndNormalized[2 * j+1]-127)/127.0+centEl));
             delayMicroseconds(DELAY_MIRRORS_CONTINUOUS); //must calibrate the timing
             _rawScanData[j] = IO.LockInRead_Volts();
            }
         }
         
         IO.setRedPower_mW(MIN_RedLaserPower);
         IO.setGreenPower(LOW);
}

int LivingSpot::scanTrajectory_Letters(byte *saccadePosCenteredAndNormalized, int numsamples, float centAz, float centEl, float radius, float rotation) {  

    
        IO.setRedPower_mW(MIN_RedLaserPower);
        IO.setGreenPower(LOW);
  
        // Set mirrors in start position (to avoid a line between the spots): 
        // We do FIRST resizing, then rotation, and then translation:
        float  x = radius*(saccadePosCenteredAndNormalized[0]-127)/127.0;
        float  y = radius*(saccadePosCenteredAndNormalized[1]-127)/127.0;
	IO.setMirrorsXY_AD(int(cos(rotation)*x-sin(rotation)*y+centAz),int(sin(rotation)*x+cos(rotation)*y+centEl));
         delayMicroseconds(DELAY_MIRRORS_START);
  
        if (PowerCorrectionMode==POWER_ADJUST) correctLaserPower(); 

	IO.setRedPower_mW(RedLaserPower); // this function (also) performs clipping
	IO.setGreenPower(GreenLaserPower);

    #ifdef NO_HARDWARE
   Serial.print('S');
 #endif	

	// Correct radius size (actually SIZE of ANY drawing!!) if required by the radius correction mode:
	if (RadiusCorrectionMode==RADIUS_ADJUST) updateRadius(); // update radius use the default R_cm_REF (which can be changed at any time by the user, and which is supposed to be the 

        register byte j;
        if (rotation!=0) {
           for (byte i=0; i<numsamples; i++) {//+EXTRA_MIRROR_SAMPLES; i++) {
             j=i%numsamples;
             // We do FIRST resizing, then rotation, and then translation:
             x = radius*(saccadePosCenteredAndNormalized[2 * j]-127)/127.0;
             y = radius*(saccadePosCenteredAndNormalized[2 * j + 1]-127)/127.0;
             // Position the mirrors: 
	     IO.setMirrorsXY_AD(int(cos(rotation)*x-sin(rotation)*y+centAz),int(sin(rotation)*x+cos(rotation)*y+centEl));
         
             // Inter point delay (the larger it is, the more the measurement will actully correspond to the sample position, and not to the arc between consecutive positions)
	    delayMicroseconds(DELAY_MIRRORS_CONTINUOUS); //must calibrate the timing
            

	    //if (i>=EXTRA_MIRROR_SAMPLES) _rawScanData[j] = IO.LockInRead_Volts(); // if to avoid calling the LockInRead on the points to be discareded
            _rawScanData[j] = IO.LockInRead_Volts(); // if to avoid calling the LockInRead on the points to be discareded
            
           }
         } else { // no rotation: this is useful for accelerating computation in the case of a circular saccade that does need to be rotated:
            for (byte i=0; i<numsamples; i++) { //+EXTRA_MIRROR_SAMPLES; i++) {
              j=i%numsamples;
             // We do FIRST resizing, then translation:
             IO.setMirrorsXY_AD(int(radius*(saccadePosCenteredAndNormalized[2 * j]-127)/127.0+centAz),int(radius*(saccadePosCenteredAndNormalized[2 * j+1]-127)/127.0+centEl));
             delayMicroseconds(DELAY_MIRRORS_CONTINUOUS); //must calibrate the timing
             _rawScanData[j] = IO.LockInRead_Volts();
            }
         }
         
         IO.setRedPower_mW(MIN_RedLaserPower);
         IO.setGreenPower(LOW);
         
}

// ====================== ANALYZE SACCADE DATA =============================
// It is assumed that the data is in the global variable _rawScanData[]
int LivingSpot::analyzeScanData(int numsamples) { // the numsamples parameter will be helpful to analyze smaller saccades, and must be always < MAX_SACCADE_SAMPLES
	//     - filter saccade data (as indicated by filter mode)
	//     - Compute "global" (i.e., per-saccade) statistics and other things 
	//     - Compute global auto-threshold
	//     - Compute global depth range (call to ComputeRange, the "constrained" function). We will modify the global variable depth_Range.
	//     - Compute aperture correction (to mantain the size of the drawing). We won't modify the current size.
	//     - NEW: PER-SAMPLE information too: in particular per-sample RANGE. 
    
        int i;

	// throw error if numsamples>MAX_SACCADE_SAMPLES
	if (numsamples>MAX_SACCADE_SAMPLES) return(-1);

	// (1) pre-process signal ----------------------------------------------------------
	if (filtering_Mode==LOW_PASS_FILTER) LowPassFilter(numsamples); // Low pass filter on the rough acquired data (only on the first numsamples)
		

	// (2) Analyze data ----------------------------------------------------------

	// (a) NEW: compute PER-SAMPLE information: 
	// Compute per-sample RANGE (using laser power updated per-saccade basis: this is a preliminary solution that may work if the saccade does not have wild variations on its intensity):
	// When computeDepthPerSample==true, the depthSaccade[] array will be filled in the computeDepthSamples (using the _rawScanData[] values, and the "global-saccade" laser power RedLaserPower:
	if (computeDepthPerSample) computeDepthSamples(RedLaserPower, numsamples); 

	// (b) Compute GLOBAL statistics (maximum, minimum, average and variance of the intensity - AFTER smoothing, filtering, etc).

	// Initialize variables that will track max and min values: 
	maxI_Volts = 0; minI_Volts = I_Volts_SAT;
	averageI_Volts=0;

	// Compute maximum and minimum, as well as average: 
        // ATTN:since 20.7.2010 for optimization purposes we do this in the scan trajectory:	
        for (i=0; i<numsamples;i++) {
		I_Volts=_rawScanData[i];
		// compute maximum, minimum and first order moment (average)
		if(I_Volts > maxI_Volts) maxI_Volts = I_Volts;
		else if(I_Volts < minI_Volts) minI_Volts = I_Volts;
		// add to average accumulator: 
		averageI_Volts +=I_Volts;
	}
        averageI_Volts/=1.0*numsamples;
        
	// Second order moment (variance), on the WHOLE saccade (it could be interesting to measure only on the white zone, sigmI_Volts_max)
	// (Note: we can disable this for maximum speed). If disabled, make sigmI_Volts a default value: 
        sigmI_Volts=0.02; // default value. 
    /*
	sigmI_Volts=0; // REM: averageI and sigmauxI are floating point variables, not integers!
	register float d;
	for(i=0; i<numsamples; i++){ 
		d = 1.0 * (_rawScanData[i] - 	averageI_Volts);
		sigmI_Volts += d * d;
	}
	sigmI_Volts = sqrt(sigmI_Volts / (numsamples-1)); // unbiased sample estimator of the variance
     */
  
            
         //Contrast flag (we will use maxI_Volts and minI_Volts).
         // This flag will be used when computing the automatic threshold: if this flag is BAD_CONSTRAST, then we can consider the saccade full inside or full outside the white zone...
        // REM: MIN_CONTRAST_Volts is computed from the max and min at calibration, multiplied by MIN_CONTRAST_FACTOR (set at the init, for instance 0.1). 
         if ((maxI_Volts-minI_Volts)>=MIN_CONTRAST_Volts) Proper_Contrast_Flag=GOOD_CONTRAST; else Proper_Contrast_Flag=BAD_CONSTRAST;
         // Note: this will affect auto-thresholding (could affect also fixed-threshold).
        
         // IN FACT, we could AVOID altoghether the thresholding if we check proper contrast before... but for the time being I don't care. 
         //if (Proper_Contrast_Flag==GOOD_CONTRAST) ...

	// COMPUTE THRESHOLDS: 
	// REM: I prefer to have fixed, auto and depth threshold as global variables, calculated unconditionnally (or modified by hand), 
	// so as to be able to check them somewhere else (behaviours), instead of calculating them here locally for binarization. 
	// (1) Compute auto_Threshold_Volts (always calculated, even if thresholding_Mode  state variable is set to FIXED_THRESHOLD - the idea is 
	// that we will be able to check the value whenever we want):
	auto_Threshold_Volts=computeAutoThreshold();//maxI_Volts, minI_Volts);
	// (2) Compute depth_Threshold_Volts, from the current depthThreshold (modified by potentiometer?):
	// this is done by a special function updateDepthThreshold()
	depth_Threshold_Volts=computeIntensityVolts(depthThreshold, RedLaserPower);

	// Binarization of saccade data depending on the threshold mode: 
         // Choice of the threshold to apply: 
	float auxThreshold;
	switch (thresholding_Mode ) {// FIXED_THRESHOLD, AUTO_THRESHOLD, DEPTH_THRESHOLD 
	  case AUTO_THRESHOLD:
		  auxThreshold=auto_Threshold_Volts; 
		  break;
	  case FIXED_THRESHOLD:
		  auxThreshold=fixed_Threshold_Volts;
		  break;
	  case DEPTH_THRESHOLD: 
		  auxThreshold=depth_Threshold_Volts;
		  // NOTE: in the future, when we will have a real phase-measurement based rangefinder, 
		  // the binarization threshold can be done directly on the per-sample depth range (depthSaccade[])
		  break;
	  default: 
		  break;
	}
        // Thresholding:
        // (rem: we can also compute the average min and max of the intensity, even on the points that may be deleted by the morphology filter?). I have opted for doing 
        // this AFTER morphological filtering (the points may be "aberrations". This is certainly true for saccades with many points - with little number of points, it is better not to do 
        // morpho-filter at all: we may render "blind" relatively large angle portions on the scanning saccade.    
	for(i=0; i<numsamples; i++) _binarizedScanData[i]=(_rawScanData[i] >= auxThreshold);

        // Morphological filter to delete solitary points (again, this is ok in case of saccades with many points, otherwise it is not a good idea). 
        if (binary_smoothing_Mode==MORPHO_SMOOTH) {// smooth data before computing the number of "light-zones"		
		_Dilate(_numSamplesCircularSaccade); // rem: dilate and erode always act on the binarizedData array, but the size of the processing batch can be modified. 
		_Erode(_numSamplesCircularSaccade);
		_Erode(_numSamplesCircularSaccade);
		_Dilate(_numSamplesCircularSaccade);
	}
        
        // Compute statistics on the saccade: 
	average_maxI_Volts=0;  average_minI_Volts=0;
	whiteProportion=0;
        int j=0;
	for(i=0; i<numsamples; i++) {
              if (_binarizedScanData[i]) {
                average_maxI_Volts+=_rawScanData[i];
                j++;
              } else average_minI_Volts+=_rawScanData[i];
        }
        if (j>0) average_maxI_Volts/=1.0*j; // otherwise leave it as 0
	if (j!=numsamples) average_minI_Volts/=1.0*(numsamples-j); // otherwise leave it as 0
	whiteProportion=1.0*j/numsamples;
	

	// ***** Choose the measured intensity I_Volts that will be used to calculate the depth_Range of the WHOLE saccade.
	// a) I_Volts can be just the MAXIMUM INTENSITY COMPUTED DURING THE SACCADE:
	// I_Volts=maxI_Volts; 
	// b) Or else, I_Volts can be the average intensity taken for the values that are LARGER than the CURRENT threshold:
	I_Volts=maxI_Volts; 
        
         // Proper intensity flag (basically, to compute depth, so we need to do that on I_Volts): 
         if (I_Volts>I_Volts_UpperMarge)  Proper_Intensity_Flag=SATURATED_SIGNAL;
         else if (I_Volts<I_Volts_LowerMarge)  Proper_Intensity_Flag=TOO_WEAK;
         else { // this means appropiate range
           Proper_Intensity_Flag=APPROPRIATE_STRENGH;

	 // "global" (saccade) DEPTH RANGE COMPUTATION (with proper limit checking, both on input I_Volts and max and min acceptable output range) 
         depth_Range=ComputeRange(I_Volts, RedLaserPower);

	// Compute the Laser power correction here?. Perhaps it is better not to do this here, since actual laser power modification 
	// is done in the _saccadeTrajectory() function depending on the power correction mode (the laser power is actually updated in per-saccade basis),
	//and this function may be time consuming. Otherwise, do: delta_RedLaserPower=LaserPowerCorrection(I_Volts, RedLaserPower);

	// Compute the correction value for the radius (whether updateRadius is called or not - it can be useful for something...). A call to updateRadius will be 
	// done if required by the radius update mode) in the SACCADE() function)
	// NOTE: in fact, the "radius" can be seen as the size of ANY figure (not only a circle!!).  
        delta_Aper_AD=computeRadiusCorrection(depth_Range); // input is the current depth_Range, output is delta_Aper_AD (rem: proper depth_Range is tested in thecomputeRadiusCorrection function using intensity contrast flat)
        }
	// everything went fine: 
	return(0);
}

// This method is particularly useful when the saccade is CIRCULAR, but we can use any saccade if we use the new angular correction computation based on the first order moment. 
// Note: In case of the circular saccade, we better do rotation parameter equal to 0 (acceleration of computation). For this, we just dont give the last parameter, and by default (see declaration) it will
// be equal to 0. 
// The recentering vector (deltaAz, deltaEl) will be computed, as well as its normalized form (nx, ny). The norm of (deltaAz, deltaEl) is actually Aper_DA (the size of the drawing!). . 
// return 1 if correction was possible and actually done on recentering vector, otherwise 0.
boolean LivingSpot::computeNormalVector(float paramAz, float paramEl, float paramAper_DA, float paramRotation) {
        boolean returnValue=false;

	// First, perform the scanning saccade: 
	scanTrajectory(_circularSaccade, _numSamplesCircularSaccade, paramAz, paramEl, paramAper_DA, 0);//paramRotation);  // perform a CIRCULAR saccade and acquire data


	// Then, analyze the data (note: data is ALWAYS on the array _rawScanData
	analyzeScanData(_numSamplesCircularSaccade); //_numSamplesCircularSaccade is the current size of the circular saccade (in the future, we will have a class "trajectory"
	// that will have a member num_samples.


	// Last, if contrast was good (or equivalently depth_Range in the proper limits), we can compute the correction translation vector: 
	if (Proper_Contrast_Flag==GOOD_CONTRAST) { 
        // Computation of the normal vector, using the latest saccade radius:
         returnValue=computeAngularCorrection();// using white/dark zone separation
        // returnValue=computeAngularCorrection_NEW(); // using first order moment calculation
          } else { // make the deltaAz and deltaEl equal to 0, as well as nx and ny
            deltaAz=0; deltaEl=0; 
            nx=0; ny=0; NormVector=0;
          }
   
	// computeAngularCorrection has set deltaEl and deltaAz (and its normalized value nx, ny)
  return(returnValue); // return 1 if correction was possible and actually done on recentering vector.
}  


// NEW (19.7.2010)!!! COMPUTE ANGULAR CORRECTION by simple discrete integration (center of gravity!!)
// TO METHODS should be available: do this directly on the continuous intensity (but how to normalize? in principle it should be ok because of the power correction), or on the 
// discrete (thresholded) saccade. As a start, let's do the last thing.
// The very nice thing about this method, besides being conceptually simpler (no problem with the topology of the buffer) are: 
// - we can use continuous values (smooth tracking?)
// - we can use saccades of arbitrary shapes, and the method won't change a bit! 
// - we can have a linear weight function that is not constant, then privileging certain directions (can be useful?)
// - It may seem that there is a drawback: we would need to compute cos and sin for each sample... but this is not necessary: it's already done in the circular saccade buffer!!! Actually the same can be be said for 
// any arbitrary trajectory! we don't need to recompute anything!!
// - We don't need to separate cases on number of "false" light/dark regions because of the linear buffer vs. circular topology of the saccade. 
// - More interestingly, we don't even have to treat separatedly the cases of "real" light/dark zones: the solution to this is very natural here: the result when there
// are several light regions is to take again the weighted average of each direction, computed on separated zones! Note however that this works well when each contiguous light zone is not larger than PI...
// In fact, if this can happen (which is mostly always the case, then we need to go back to the assumption of a unique light region, and detect the case >PI (by just checking that whiteProportion<0.5). A more
// complete test would compute the angular span of each light region, compute the correction for it (depending on > or< PI), and then summing those up. 
// OUTPUT: updated variables: (deltaEl, deltaAz) with norm NormVector and argument target_angle, and its normalized value (nx, ny)=(deltaEl, deltaAz)/NormVector
// Return: return 1 if correction is needed, and 0 if nothing is to be done (this is a little artificial in this method: the computation is exacly the same. The return value will be done by comparing
// NormVector to some pre-defined minimum (perhaps 0, but beware of the float precision). 
boolean LivingSpot::computeAngularCorrection_NEW() {// float *saccadePosCenteredAndNormalized, int numsamples, float radius) {
  boolean returnValue;
  
float Xg=0, Yg=0; // the center of gravity (first order moment): 
float normOG; // the norm of the vector [Xg, Yg]
float delta_angle=0; // the angular extension of the white region
// Integration (computation of the first order moment):
// NOTE: for the time being, the linear weight won't be the intensity, but the thresholded values: 
for (int i=0; i<_numSamplesCircularSaccade; i++) {
  if(_binarizedScanData[i]) {
    Xg+=1.0*(_circularSaccade[2*i]-127)/127.0; 
    Yg+=1.0*(_circularSaccade[2*i+1]-127)/127.0;
  }
}
normOG=sqrt(Xg*Xg+Yg*Yg); // if this is 0, it means that the saccade is either completely on the white or dark zone, so let's make nx and ny=0: 
if (normOG==0) {
  delta_angle=PI;
  NormVector=0;
  nx=0; ny=0;
  deltaAz=0; deltaEl=0;
  returnValue=0;
}
else { 
  //Xg/=2.0*PI/numsamples; // remember that the sum was the approximation of the integral (this is equal to the R.dtheta, but R=1). BUT WE DON'T CARE, we only want the DIRECTION of this vectorial summation!!
  //Yg/=2.0*PI/numsamples;
 // Now, in case of a CIRCULAR saccade, we have (see calculations 19.7.2010):
// (a) The target angle:  IN FACT WE DON'T NEED IT, unless we do angular correction (but even in that case, it is better to ROTATE the vector, instead of computing atan?)
 // if (Xg!=0) target_angle=atan2(Yg,Xg); else target_angle=PI*(Xg>0);
 // => angular portion on the light zone (We want NormVector to be proportional to that). 
  //     If  whiteProportion<0.5, we have delta_angle=2*asin(normOG/2) (if we computed values on a circle of radius 1). BUT if we assume only ONE white region, then whiteproportion is proportional to delta_angle!!
  //     If  whiteProportion>0.5 ...
  delta_angle=2*PI*whiteProportion; // max can be 2*Aper_DA
  NormVector= Aper_DA * (1.0 + cos(delta_angle/2)); // norm of the angular repositioning vector (max can be 2*Aper_DA)
  // Finally, compute the recentering vector: 
  // using the target angle: 
  //nx = cos(target_angle + error_angle); 
  //ny = sin(target_angle + error_angle);
  // .. or better: directly from Xg, Yg (no need to compute atan and then cos and sin!):
  nx=Xg/normOG; 
  ny=Yg/normOG;
  // ATTN! we need to rotate the notmal vector by the error_angle:
  float auxc=cos(error_angle), auxs=sin(error_angle); 
  deltaAz=auxc*nx-auxs*ny;
  ny=auxs*nx+auxc*ny; 
  nx=deltaAz; //swap (this was just to avoid another variable). 
  deltaAz = NormVector * nx;
  deltaEl = NormVector * ny;
  returnValue=1;
}
return(returnValue);
}

// This function computes the correction vector deltaEl and deltaAz (and its normalized value nx, ny)
// REM: it is only applicable to circular saccades. It should be in fact a method of the behaviour class computeNormalVector...
// It process _binarizedScanData, calculated in the analyzeScanData() function, and we assume here that the size is that of the global 
// variable _num_Samples (again, the size of a particular saccade should be a member function). 
// OUTPUT: updated variables: (deltaEl, deltaAz) with norm NormVector and argument target_angle, and its normalized value (nx, ny)=(deltaEl, deltaAz)/NormVector
boolean LivingSpot::computeAngularCorrection() { // return 1 if correction is needed, and 0 if nothing is to be done (or cannot be done)
	int i; // counter
	int returnValue=0;

	//initialize normal vector:
	deltaAz=0; deltaEl=0;

	int for_averaging=0; // number of samples for averaging the target_angle
	boolean zone_flag=false; // flag to help counting the number of lighted/dark regions
	int light_counter=0, dark_counter=0; // region counters.
	int min_angle=0, max_angle=_numSamplesCircularSaccade-1; // default value of the angles (in samples) of the considered region
	float delta_angle=0; // values of the angular extension of the considered region
	boolean flag_min_angle=true; // flag for min_angle and max_angle calculation

	// float NormVectorMultiplier=1; // Multiplier of NormVector [a public variable now]
	// float NormVector; // Norme du vecteur de deplacement ANGULAIRE! [a public variable now]

	// 5): compute the number of lighted/dark regions.
	// REM: the number of items on _binarizedScanData array are just equal to the number of points (numSamples)
	zone_flag=_binarizedScanData[0];
	for (i=0; i<_numSamplesCircularSaccade; i++) {
		if (zone_flag == _binarizedScanData[i]) {
			if (zone_flag)
				light_counter++;
			else
				dark_counter++;
			zone_flag =!zone_flag;
		}
	}


	// 6) calculation of the "target" angle depending on the number of lighted/dark regions
	// 0 -  NO lighted region (target is lost), nothing is done
	// 1 - one and only one lighted region, average angle is calculated on the lighted zone
	// Nothing is done if only lighted region (no dark region). Two dark regions possible.
	// 2.1 - two lighted regions and one and only one dark region:
	// average angle is calculated on the dark zone, and it should be added PI radians.
	// 2.2 - two lighted regions and two dark regions:
	// average angle is calculated on the second lighted zone.
	// 3 (default) - Other cases : nothing is done yet.	

	target_angle=0; // direction of displacement (by doing =0, we initialize target angle "accumulator")
	switch (light_counter) 
	{
	case 0: // nothing is done (saccade full in dark zone)
		return 0;
		break;
	case 1: // unique lighted region
		i=0;
		while(!_binarizedScanData[i]) i++; // skip the dark zone
                //min_angle = i; ??? 
		while((i<_numSamplesCircularSaccade) && (_binarizedScanData[i])){ // we are on the unique ligthed zone
			target_angle = target_angle + i;
			for_averaging++;
			if(flag_min_angle){	
				flag_min_angle = false; 
				min_angle = i;
			}
			i++;
		}
		max_angle=i-1;// max_angle contains the index of the last illuminated sample

		if((max_angle-min_angle) != (_numSamplesCircularSaccade - 1)){ //otherwise it means no correction is needed: the saccade is all inside the light zone
			delta_angle = 2.0 * PI / _numSamplesCircularSaccade * (max_angle-min_angle); // angular extension INSIDE the lighted zone
			NormVector = Aper_DA * (1 + cos(delta_angle/2)); // norm of the angular repositioning vector (max can be 2*Aper_DA)
			//target_angle=1.0*(max_angle-min_angle)*PI/numSamples;
			target_angle = target_angle / for_averaging * 2.0 * PI / _numSamplesCircularSaccade; // direction angle	in radians. 
			//target_angle+=2*PI/numSamples*mirror_delay_Samples+teta0;
			// The corrective angles are therefore:
			nx = cos(target_angle + error_angle); 
			ny = sin(target_angle + error_angle);
			deltaAz = NormVector * nx;
			deltaEl = NormVector * ny;

			returnValue=1;
		}
		break;
	case 2: // two lighted regions
		if(dark_counter == 1) { // one and only one dark region
			i=0;
			while(_binarizedScanData[i] == 1) i++; // skip first lighted zone
			while((i<_numSamplesCircularSaccade) && (_binarizedScanData[i]==0)){ // we are on the unique dark zone
				target_angle = target_angle + i;
				for_averaging++;
				if (flag_min_angle) {	
					flag_min_angle = false; 
					min_angle = i;
				}
				i++;
			}
			max_angle = i-1;// max_angle contains the index of the last dark sample

			delta_angle = 2.0 * PI / _numSamplesCircularSaccade * (max_angle-min_angle); // angular extension
			NormVector = Aper_DA * (1 - cos(delta_angle/2)); // new repositioning vector:
			// addition of PI radians is necessary when estimating the target_angle in the dark zone:
			//target_angle=1.0*(max_angle-min_angle)*PI/numSamples+PI;
			target_angle = target_angle / for_averaging * 2.0 * PI / _numSamplesCircularSaccade; // direction angle	
			//target_angle+=2*PI/numSamples*mirror_delay_Samples+teta0;
			// the corrective angles:
			nx = -cos(target_angle + error_angle); 
			ny = -sin(target_angle + error_angle);
			deltaAz = NormVector * nx;
			deltaEl = NormVector * ny;

			returnValue=1;
		}
		else{
			//not done yet
		}
		break;
	default: // more than two lighted regions: nothing is done
		break;
	} // end of switch case	
	return(returnValue);
}


// Linear Filter (to smooth data BEFORE binarization):
int LivingSpot::LowPassFilter(int numsamples) {// Low pass filter on the rough acquired data (AD units).
// NOTE: (20.7.2010): to gain RAM SPACE, I use depthSaccade[] as the auxiliary array. This is possible because we always 
// filter FIRST (if required), and then (if required) compute per-sample depth. float auxDataProcessing[MAX_SACCADE_SAMPLES];
	if (numsamples>MAX_SACCADE_SAMPLES) return (-1);

	float filter_Impulse_Response[5]={0.2/2.6, 0.6/2.6, 1/2.6, 0.6/2.6, 0.2/2.6}; // rem: it must be normalized, so as not to change average
	int i,j,k; //counter
	// rem: auxDataProcessing[MAX_SACCADE_SAMPLES] is an auxiliary global array
	// rem: data is considered to lie in a "circle", i.e., there are no edges to be treated in a special way.
	for (i=0; i<numsamples;i++) {
		depthSaccade[i]=0;
		for (j=0;j<5;j++) {
			k=(i+j-2+numsamples)%numsamples; //rem: k must be positive, and C++ implementation of modulus (%) is ambiguous if one operator is negative
			// hence the +numSamples...
			depthSaccade[i]+=_rawScanData[k]*filter_Impulse_Response[j]; // rem: % is the modulus function
		}
	}
	for (i=0; i<numsamples;i++) _rawScanData[i]=depthSaccade[i];
	return(0);
}


// -------------------------- MORPHOLOGIC filters for smoothing binary data:
// Again, the binary data to be analyzed is assumed to be ALWAYS in _binarizedScanData
// Also, its topology is assumed to be circular (i.e., no borders). 
void LivingSpot::_Dilate(int numsamples) { // Dilation 
	//if (numsamples>MAX_SACCADE_SAMPLES) return (-1);

	int i; //counter
	_auxBinarizedScanData[0] = _binarizedScanData[numsamples-1] | _binarizedScanData[0] | _binarizedScanData[1];
	for (i=1; i<numsamples-1;i++) _auxBinarizedScanData[i] = _binarizedScanData[i-1] | _binarizedScanData[i] | _binarizedScanData[i+1];
	_auxBinarizedScanData[numsamples-1] = _binarizedScanData[numsamples-2] | _binarizedScanData[numsamples-1] | _binarizedScanData[0];
	for (i=0; i<numsamples;i++) _binarizedScanData[i] = _auxBinarizedScanData[i];
 //return(0);
}

void LivingSpot::_Erode(int numsamples) { // Erosion
	//if (numsamples>MAX_SACCADE_SAMPLES) return (-1);
	
        // rem: data is considered to lie in a "circle", i.e., there are no edges to be treated in a special way.
	int i; //counter
	_auxBinarizedScanData[0] = _binarizedScanData[numsamples-1] & _binarizedScanData[0] & _binarizedScanData[1];
	for (i=1; i<numsamples-1;i++) _auxBinarizedScanData[i] = _binarizedScanData[i-1] & _binarizedScanData[i] & _binarizedScanData[i+1];
	_auxBinarizedScanData[numsamples-1] = _binarizedScanData[numsamples-2] & _binarizedScanData[numsamples-1] & _binarizedScanData[0];
	for (i=0; i<numsamples;i++) _binarizedScanData[i] = _auxBinarizedScanData[i];
  //return(0);
}
// ------------------------------------------------------------------------------


